Merge branch 'main' into feat/add_lidar_marker_localizer

.github/labeler.yaml

@@ -31,7 +31,7 @@
   - planning/**/*
 "component:sensing":
   - sensing/**/*
-"component:simulator":
+"component:simulation":
   - simulator/**/*
 "component:system":
   - system/**/*

.github/sync-files.yaml

@@ -12,8 +12,6 @@
     - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md
     - source: .github/dependabot.yaml
     - source: .github/stale.yml
-      pre-commands: |
-        sd "staleLabel: stale" "staleLabel: status:stale" {source}
     - source: .github/workflows/cancel-previous-workflows.yaml
     - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -29,6 +29,7 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <boost/geometry/index/rtree.hpp>
 #include <boost/optional.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -46,7 +47,9 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using autoware_planning_msgs::msg::LaneletRoute;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::PoseDeviation;
+using tier4_autoware_utils::Segment2d;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
+typedef boost::geometry::index::rtree<Segment2d, boost::geometry::index::rstar<16>> SegmentRtree;
 
 struct Param
 {
@@ -137,13 +140,15 @@ class LaneDepartureChecker
     const lanelet::ConstLanelets & candidate_lanelets,
     const std::vector<LinearRing2d> & vehicle_footprints);
 
+  double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const;
+
+  static SegmentRtree extractUncrossableBoundaries(
+    const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
+    const double max_search_length, const std::vector<std::string> & boundary_types_to_detect);
+
   static bool willCrossBoundary(
-    const lanelet::ConstLanelets & candidate_lanelets,
     const std::vector<LinearRing2d> & vehicle_footprints,
-    const std::vector<std::string> & boundary_types_to_detects);
-
-  static bool isCrossingRoadBorder(
-    const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints);
+    const SegmentRtree & uncrossable_segments);
 };
 }  // namespace lane_departure_checker
 

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -30,7 +30,9 @@
 #include <algorithm>
 #include <vector>
 
+using motion_utils::calcArcLength;
 using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::MultiPoint2d;
 using tier4_autoware_utils::Point2d;
 
@@ -41,15 +43,6 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using geometry_msgs::msg::Point;
 
-Point fromVector2dToMsg(const Eigen::Vector2d & point)
-{
-  Point msg{};
-  msg.x = point.x();
-  msg.y = point.y();
-  msg.z = 0.0;
-  return msg;
-}
-
 double calcBrakingDistance(
   const double abs_velocity, const double max_deceleration, const double delay_time)
 {
@@ -67,27 +60,6 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2
   return false;
 }
 
-bool isCrossingWithBoundary(
-  const lanelet::BasicLineString2d & boundary, const std::vector<LinearRing2d> & footprints)
-{
-  for (auto & footprint : footprints) {
-    for (size_t i = 0; i < footprint.size() - 1; ++i) {
-      auto footprint1 = footprint.at(i).to_3d();
-      auto footprint2 = footprint.at(i + 1).to_3d();
-      for (size_t i = 0; i < boundary.size() - 1; ++i) {
-        auto boundary1 = boundary.at(i);
-        auto boundary2 = boundary.at(i + 1);
-        if (tier4_autoware_utils::intersect(
-              tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2),
-              fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) {
-          return true;
-        }
-      }
-    }
-  }
-  return false;
-}
-
 LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprints)
 {
   MultiPoint2d combined;
@@ -172,8 +144,12 @@ Output LaneDepartureChecker::update(const Input & input)
   output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front());
   output.processing_time_map["isOutOfLane"] = stop_watch.toc(true);
 
-  output.will_cross_boundary = willCrossBoundary(
-    output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect);
+  const double max_search_length_for_boundaries =
+    calcMaxSearchLengthForBoundaries(*input.predicted_trajectory);
+  const auto uncrossable_boundaries = extractUncrossableBoundaries(
+    *input.lanelet_map, input.predicted_trajectory->points.front().pose.position,
+    max_search_length_for_boundaries, input.boundary_types_to_detect);
+  output.will_cross_boundary = willCrossBoundary(output.vehicle_footprints, uncrossable_boundaries);
   output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true);
 
   return output;
@@ -334,38 +310,58 @@ bool LaneDepartureChecker::isOutOfLane(
   return false;
 }
 
-bool LaneDepartureChecker::willCrossBoundary(
-  const lanelet::ConstLanelets & candidate_lanelets,
-  const std::vector<LinearRing2d> & vehicle_footprints,
-  const std::vector<std::string> & boundary_types_to_detect)
+double LaneDepartureChecker::calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const
 {
-  for (const auto & candidate_lanelet : candidate_lanelets) {
-    const std::string r_type =
-      candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none");
-    if (
-      std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) !=
-      boundary_types_to_detect.end()) {
-      if (isCrossingWithBoundary(
-            candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) {
-        // std::cerr << "The crossed road_border's line string id: "
-        //           << candidate_lanelet.rightBound().id() << std::endl;
-        return true;
+  const double max_ego_lon_length = std::max(
+    std::abs(vehicle_info_ptr_->max_longitudinal_offset_m),
+    std::abs(vehicle_info_ptr_->min_longitudinal_offset_m));
+  const double max_ego_lat_length = std::max(
+    std::abs(vehicle_info_ptr_->max_lateral_offset_m),
+    std::abs(vehicle_info_ptr_->min_lateral_offset_m));
+  const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length);
+  return calcArcLength(trajectory.points) + max_ego_search_length;
+}
+
+SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries(
+  const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
+  const double max_search_length, const std::vector<std::string> & boundary_types_to_detect)
+{
+  const auto has_types =
+    [](const lanelet::ConstLineString3d & ls, const std::vector<std::string> & types) {
+      constexpr auto no_type = "";
+      const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type);
+      return (type != no_type && std::find(types.begin(), types.end(), type) != types.end());
+    };
+
+  SegmentRtree uncrossable_segments_in_range;
+  LineString2d line;
+  const auto ego_p = Point2d{ego_point.x, ego_point.y};
+  for (const auto & ls : lanelet_map.lineStringLayer) {
+    if (has_types(ls, boundary_types_to_detect)) {
+      line.clear();
+      for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()});
+      for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) {
+        const Segment2d segment = {line[segment_idx], line[segment_idx + 1]};
+        if (boost::geometry::distance(segment, ego_p) < max_search_length) {
+          uncrossable_segments_in_range.insert(segment);
+        }
       }
     }
-    const std::string l_type =
-      candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none");
-    if (
-      std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) !=
-      boundary_types_to_detect.end()) {
-      if (isCrossingWithBoundary(
-            candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) {
-        // std::cerr << "The crossed road_border's line string id: "
-        //           << candidate_lanelet.leftBound().id() << std::endl;
-        return true;
-      }
+  }
+  return uncrossable_segments_in_range;
+}
+
+bool LaneDepartureChecker::willCrossBoundary(
+  const std::vector<LinearRing2d> & vehicle_footprints, const SegmentRtree & uncrossable_segments)
+{
+  for (const auto & footprint : vehicle_footprints) {
+    std::vector<Segment2d> intersection_result;
+    uncrossable_segments.query(
+      boost::geometry::index::intersects(footprint), std::back_inserter(intersection_result));
+    if (!intersection_result.empty()) {
+      return true;
     }
   }
   return false;
 }
-
 }  // namespace lane_departure_checker

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -33,4 +33,12 @@
       <arg name="out_accel" value="/localization/acceleration"/>
     </include>
   </group>
+
+  <group>
+    <include file="$(find-pkg-share pose_instability_detector)/launch/pose_instability_detector.launch.xml">
+      <arg name="input_odometry" value="/localization/kinematic_state"/>
+      <arg name="input_twist" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="param_file" value="$(find-pkg-share pose_instability_detector)/config/pose_instability_detector.param.yaml"/>
+    </include>
+  </group>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -25,6 +25,7 @@
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
+  <exec_depend>pose_instability_detector</exec_depend>
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>yabloc_common</exec_depend>
   <exec_depend>yabloc_image_processing</exec_depend>

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -50,6 +50,7 @@
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
   <arg name="use_near_radar_fusion" default="false"/>
   <arg name="far_object_merger_sync_queue_size" default="20"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Filter output name. Switch output topic name by 'use_radar_tracking_fusion' parameter defined in perception.launch -->
   <let name="output_of_filtered_objects" value="$(var output/objects)" if="$(var use_radar_tracking_fusion)"/>
@@ -78,9 +79,7 @@
       <arg name="filter/velocity_threshold" value="3.0"/>
       <arg name="split/velocity_threshold" value="4.5"/>
       <arg name="split_range" value="70.0"/>
-      <arg name="clustering/angle_threshold" value="0.174"/>
-      <arg name="clustering/distance_threshold" value="10.0"/>
-      <arg name="clustering/velocity_threshold" value="4.0"/>
+      <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -32,6 +32,7 @@
   <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Camera-LiDAR fusion parameters -->
   <arg name="remove_unknown" default="true"/>
@@ -145,6 +146,7 @@
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="output/objects" value="objects"/>
       <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+      <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>
 </launch>

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml

@@ -11,6 +11,7 @@
   <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- LiDAR detection-->
   <group>
@@ -37,9 +38,7 @@
       <arg name="filter/velocity_threshold" value="3.0"/>
       <arg name="split/velocity_threshold" value="4.5"/>
       <arg name="split_range" value="70.0"/>
-      <arg name="clustering/angle_threshold" value="0.174"/>
-      <arg name="clustering/distance_threshold" value="10.0"/>
-      <arg name="clustering/velocity_threshold" value="4.0"/>
+      <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml

@@ -6,10 +6,8 @@
   <arg name="filter/velocity_threshold" default="1.5"/>
   <arg name="split/velocity_threshold" default="5.5"/>
   <arg name="split_range" default="80.0"/>
-  <arg name="clustering/angle_threshold" default="0.349"/>
-  <arg name="clustering/distance_threshold" default="10.0"/>
-  <arg name="clustering/velocity_threshold" default="5.0"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Detection for far dynamic objects -->
   <include file="$(find-pkg-share radar_crossing_objects_noise_filter)/launch/radar_crossing_objects_noise_filter.launch.xml">
@@ -43,14 +41,6 @@
   <include file="$(find-pkg-share radar_object_clustering)/launch/radar_object_clustering.launch.xml">
     <arg name="input/objects" value="lanelet_filtered_objects"/>
     <arg name="output/objects" value="$(var output/objects)"/>
-    <arg name="angle_threshold" value="$(var clustering/angle_threshold)"/>
-    <arg name="distance_threshold" value="$(var clustering/distance_threshold)"/>
-    <arg name="velocity_threshold" value="$(var clustering/velocity_threshold)"/>
-    <arg name="is_fixed_label" value="true"/>
-    <arg name="fixed_label" value="CAR"/>
-    <arg name="is_fixed_size" value="true"/>
-    <arg name="size_x" value="4.0"/>
-    <arg name="size_y" value="1.5"/>
-    <arg name="size_z" value="1.5"/>
+    <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
   </include>
 </launch>

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -12,6 +12,7 @@
   <arg name="object_recognition_detection_fusion_sync_param_path"/>
   <arg name="object_recognition_detection_lidar_model_param_path"/>
   <arg name="object_recognition_detection_radar_lanelet_filtering_range_param"/>
+  <arg name="object_recognition_detection_radar_object_clustering_param_path"/>
   <arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"/>
@@ -103,7 +104,7 @@
   <!-- Radar long range integration methods -->
   <arg
     name="use_radar_tracking_fusion"
-    default="false"
+    default="true"
     description="if use_radar_tracking_fusion:=true, radar information is merged in tracking launch. Otherwise, radar information is merged in detection launch."
   />
 
@@ -175,7 +176,9 @@
           <arg name="outlier_param_path" value="$(var object_recognition_detection_outlier_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="radar_lanelet_filtering_range_param" value="$(var object_recognition_detection_radar_lanelet_filtering_range_param)"/>
+          <arg name="radar_object_clustering_param_path" value="$(var object_recognition_detection_radar_object_clustering_param_path)"/>
           <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
+
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>

launch/tier4_perception_launch/package.xml

@@ -39,6 +39,7 @@
   <exec_depend>shape_estimation</exec_depend>
   <exec_depend>tensorrt_yolo</exec_depend>
   <exec_depend>topic_tools</exec_depend>
+  <exec_depend>tracking_object_merger</exec_depend>
   <exec_depend>traffic_light_arbiter</exec_depend>
   <exec_depend>traffic_light_classifier</exec_depend>
   <exec_depend>traffic_light_fine_detector</exec_depend>

localization/geo_pose_projector/src/geo_pose_projector.cpp

@@ -36,7 +36,8 @@ GeoPoseProjector::GeoPoseProjector()
 
   // Subscribe to geo_pose topic
   geo_pose_sub_ = create_subscription<GeoPoseWithCovariance>(
-    "input_geo_pose", 10, [this](const GeoPoseWithCovariance::SharedPtr msg) { on_geo_pose(msg); });
+    "input_geo_pose", 10,
+    [this](const GeoPoseWithCovariance::ConstSharedPtr msg) { on_geo_pose(msg); });
 
   // Publish pose topic
   pose_pub_ = create_publisher<PoseWithCovariance>("output_pose", 10);
@@ -49,7 +50,7 @@ GeoPoseProjector::GeoPoseProjector()
   }
 }
 
-void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg)
+void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg)
 {
   if (!projector_info_) {
     RCLCPP_WARN_THROTTLE(

localization/geo_pose_projector/src/geo_pose_projector.hpp

@@ -39,7 +39,7 @@ class GeoPoseProjector : public rclcpp::Node
   GeoPoseProjector();
 
 private:
-  void on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg);
+  void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg);
 
   component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
   rclcpp::Subscription<GeoPoseWithCovariance>::SharedPtr geo_pose_sub_;

localization/landmark_based_localizer/ar_tag_based_localizer/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>aruco</depend>
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>