diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e95f4d46a147b..f25c2fe07aab5 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -16,7 +16,7 @@ jobs: build-and-test-differential: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: [self-hosted, linux, X64] + runs-on: ubuntu-latest container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -70,7 +70,7 @@ jobs: flags: differential clang-tidy-differential: - runs-on: [self-hosted, linux, X64] + runs-on: ubuntu-latest container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda needs: build-and-test-differential steps: 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 bd75d8af12fc9..50b73709cc902 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 @@ -31,8 +31,6 @@ - - @@ -75,11 +73,10 @@ - - + + - @@ -177,9 +174,10 @@ + + - @@ -238,9 +236,10 @@ + + - 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 184caf4e6224b..b09684281d33a 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 @@ -2,8 +2,7 @@ - - + @@ -14,8 +13,7 @@ - - + @@ -35,12 +33,12 @@ - + - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 59475afbafd92..0d517ced97d17 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -14,7 +14,8 @@ - + + @@ -189,7 +190,7 @@ - + diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index 81b1daabaa7ef..6f1270c656bde 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml b/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml new file mode 100644 index 0000000000000..f52edf641fb52 --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + angle_threshold: 1.2210 + velocity_threshold: 1.5 diff --git a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml b/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml index 5c414f37a36a4..25856e12c3ba5 100644 --- a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml +++ b/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml @@ -5,15 +5,13 @@ - - + - - + diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml index 68fca44bcc723..1759c17a5ab6b 100644 --- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml +++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -1,15 +1,12 @@ - - - - + - + diff --git a/perception/radar_tracks_msgs_converter/CMakeLists.txt b/perception/radar_tracks_msgs_converter/CMakeLists.txt index 2a7090eebb996..766fb4b939904 100644 --- a/perception/radar_tracks_msgs_converter/CMakeLists.txt +++ b/perception/radar_tracks_msgs_converter/CMakeLists.txt @@ -24,5 +24,6 @@ endif() ## Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml new file mode 100644 index 0000000000000..5a30a6bb4031d --- /dev/null +++ b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + use_twist_compensation: true + use_twist_yaw_compensation: false + static_object_speed_threshold: 1.0 diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index cc2bb80c6f4f7..1aa4d51703b0c 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -3,19 +3,13 @@ - - - - + - - - - + diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt index 77b10c0afb87a..10bd0efa7b8fc 100644 --- a/perception/simple_object_merger/CMakeLists.txt +++ b/perception/simple_object_merger/CMakeLists.txt @@ -25,5 +25,6 @@ endif() # Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 4c037963361a1..ec3c03424da52 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -72,5 +72,19 @@ If the time difference between the first topic of `input_topics` and an input to - Default parameter: "[]" This parameter is the name of input topics. -For example, when this packages use for radar objects, `"[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"` can be set. +For example, when this packages use for radar objects, + +```yaml +input_topics: + [ + "/sensing/radar/front_center/detected_objects", + "/sensing/radar/front_left/detected_objects", + "/sensing/radar/rear_left/detected_objects", + "/sensing/radar/rear_center/detected_objects", + "/sensing/radar/rear_right/detected_objects", + "/sensing/radar/front_right/detected_objects", + ] +``` + +can be set in config yaml file. For now, the time difference is calculated by the header time between the first topic of `input_topics` and the input topics, so the most important objects to detect should be set in the first of `input_topics` list. diff --git a/perception/simple_object_merger/config/simple_object_merger.param.yaml b/perception/simple_object_merger/config/simple_object_merger.param.yaml new file mode 100644 index 0000000000000..b285a3110d22d --- /dev/null +++ b/perception/simple_object_merger/config/simple_object_merger.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + new_frame_id: "base_link" + timeout_threshold: 1.0 + input_topics: [""] diff --git a/perception/simple_object_merger/launch/simple_object_merger.launch.xml b/perception/simple_object_merger/launch/simple_object_merger.launch.xml index ccd038c23b257..43e22ea081feb 100644 --- a/perception/simple_object_merger/launch/simple_object_merger.launch.xml +++ b/perception/simple_object_merger/launch/simple_object_merger.launch.xml @@ -1,18 +1,9 @@ - - - - - - + - - - - - + 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 0dccceb1e3919..90ce99692e57a 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 @@ -164,6 +164,7 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + bool isOverlapWithCenterLane() const; bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; 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 135a0b7fbf86c..f5f0e514ce8bc 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 @@ -21,6 +21,7 @@ #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include @@ -211,7 +212,8 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { - return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; + return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && + !isOverlapWithCenterLane(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -278,6 +280,37 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } +bool StartPlannerModule::isOverlapWithCenterLane() const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + for (const auto & current_lane : current_lanes) { + std::vector centerline_points; + for (const auto & point : current_lane.centerline()) { + geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); + centerline_points.push_back(center_point); + } + + for (size_t i = 0; i < centerline_points.size() - 1; ++i) { + const auto & p1 = centerline_points.at(i); + const auto & p2 = centerline_points.at(i + 1); + for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { + const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); + const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + + if (intersection.has_value()) { + return true; + } + } + } + } + return false; +} + bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -935,8 +968,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, std::numeric_limits::max()); - // Set the maximum backward distance less than the distance from the vehicle's base_link to the - // lane's rearmost point to prevent lane departure. + // Set the maximum backward distance less than the distance from the vehicle's base_link to + // the lane's rearmost point to prevent lane departure. const double current_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length; const double allowed_backward_distance = std::clamp( @@ -1224,8 +1257,8 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; - // Check if the goal and ego are in the same route segment. If not, this is out of scope of this - // function. Return false. + // Check if the goal and ego are in the same route segment. If not, this is out of scope of + // this function. Return false. lanelet::ConstLanelet ego_lanelet; rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet); const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet); diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 050c4af55c9bf..3bdfd67a7bec4 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -1548,6 +1548,8 @@ void ObstacleStopPlannerNode::filterObstacles( const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, PredictedObjects & filtered_objects) { + if (traj.size() < 2) return; + filtered_objects.header = input_objects.header; for (auto & object : input_objects.objects) { diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index 133a057cf1a12..56cbdb98fdbc8 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -43,5 +43,6 @@ endif() # Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml b/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml new file mode 100644 index 0000000000000..0d7ddd345b03d --- /dev/null +++ b/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_y_threshold: 7.0 diff --git a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml index f38fe36b50af4..028ca5de6a220 100644 --- a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml +++ b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml @@ -1,17 +1,13 @@ - - - - + - - +