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 @@
-
-
-
-
+
-
-
+