Merge branch 'main' into refactor/image_projection_based_fusion/rename_template_argument

Author: kminoda

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -121,7 +121,7 @@ class LaneDepartureChecker
   std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
-  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+  std::optional<tier4_autoware_utils::Polygon2d> getFusedLaneletPolygonForPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
   bool checkPathWillLeaveLane(

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -321,33 +321,39 @@ std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLanele
     lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
 }
 
-std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+std::optional<tier4_autoware_utils::Polygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
   const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
 {
   const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
+  auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d {
+    tier4_autoware_utils::Polygon2d p;
+    auto & outer = p.outer();
+
+    for (const auto & p : poly) {
+      tier4_autoware_utils::Point2d p2d(p.x(), p.y());
+      outer.push_back(p2d);
+    }
+    boost::geometry::correct(p);
+    return p;
+  };
+
   // Fuse lanelets into a single BasicPolygon2d
-  auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
+  auto fused_lanelets = [&]() -> std::optional<tier4_autoware_utils::Polygon2d> {
     if (lanelets_distance_pair.empty()) return std::nullopt;
-    if (lanelets_distance_pair.size() == 1)
-      return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+    tier4_autoware_utils::MultiPolygon2d lanelet_unions;
+    tier4_autoware_utils::MultiPolygon2d result;
 
-    lanelet::BasicPolygon2d merged_polygon =
-      lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
-    for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
+    for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) {
       const auto & route_lanelet = lanelets_distance_pair.at(i).second;
-      const auto & poly = route_lanelet.polygon2d().basicPolygon();
-
-      std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
-      boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
-
-      // Update merged_polygon by accumulating all merged results
-      merged_polygon.clear();
-      for (const auto & temp_poly : lanelet_union_temp) {
-        merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
-      }
+      const auto & p = route_lanelet.polygon2d().basicPolygon();
+      tier4_autoware_utils::Polygon2d poly = to_polygon2d(p);
+      boost::geometry::union_(lanelet_unions, poly, result);
+      lanelet_unions = result;
+      result.clear();
     }
-    if (merged_polygon.empty()) return std::nullopt;
-    return merged_polygon;
+
+    if (lanelet_unions.empty()) return std::nullopt;
+    return lanelet_unions.front();
   }();
 
   return fused_lanelets;

evaluator/perception_online_evaluator/package.xml

@@ -21,6 +21,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
   <depend>libgoogle-glog-dev</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

@@ -79,7 +79,9 @@ class EvalTest : public ::testing::Test
 
     marker_sub_ = rclcpp::create_subscription<MarkerArray>(
       eval_node, "perception_online_evaluator/markers", 10,
-      [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; });
+      [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) {
+        has_received_marker_ = true;
+      });
     uuid_ = generateUUID();
   }
 

localization/localization_util/src/smart_pose_buffer.cpp

@@ -40,6 +40,7 @@ std::optional<SmartPoseBuffer::InterpolateResult> SmartPoseBuffer::interpolate(
     const rclcpp::Time time_last = pose_buffer_.back()->header.stamp;
 
     if (target_ros_time < time_first) {
+      RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp");
       return std::nullopt;
     }
 

localization/localization_util/src/util_func.cpp

@@ -241,7 +241,7 @@ void output_pose_with_cov_to_log(
   rpy.y = rpy.y * 180.0 / M_PI;
   rpy.z = rpy.z * 180.0 / M_PI;
 
-  RCLCPP_INFO_STREAM(
+  RCLCPP_DEBUG_STREAM(
     logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << ","
                        << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y
                        << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -182,7 +182,7 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
   const auto duration_micro_sec =
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
-  RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
+  RCLCPP_DEBUG(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
   return true;  // Updated
 }
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -992,7 +992,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
   result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose;
 
   output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);
-  RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score);
+  RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score);
 
   return result_pose_with_cov_msg;
 }

localization/pose_initializer/src/pose_initializer/ndt_module.cpp

@@ -40,7 +40,6 @@ PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped
   RCLCPP_INFO(logger_, "Call NDT align server.");
   const auto res = cli_align_->async_send_request(req).get();
   if (!res->success) {
-    RCLCPP_INFO(logger_, "NDT align server failed.");
     throw ServiceException(
       Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed.");
   }

perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp

@@ -20,6 +20,8 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -61,6 +63,11 @@ class Debugger
     divided_objects_pub_ =
       node->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
         "debug/divided_objects", 1);
+    processing_time_publisher_ =
+      std::make_unique<tier4_autoware_utils::DebugPublisher>(node, "detection_by_tracker");
+    stop_watch_ptr_ =
+      std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+    this->startStopWatch();
   }
 
   ~Debugger() {}
@@ -80,6 +87,19 @@ class Debugger
   {
     divided_objects_pub_->publish(removeFeature(input));
   }
+  void startStopWatch()
+  {
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+  void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); }
+  void publishProcessingTime()
+  {
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
+  }
 
 private:
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
@@ -90,6 +110,9 @@ class Debugger
     merged_objects_pub_;
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     divided_objects_pub_;
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   autoware_auto_perception_msgs::msg::DetectedObjects removeFeature(
     const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input)

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -206,6 +206,7 @@ void DetectionByTracker::setMaxSearchRange()
 void DetectionByTracker::onObjects(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg)
 {
+  debugger_->startMeasureProcessingTime();
   autoware_auto_perception_msgs::msg::DetectedObjects detected_objects;
   detected_objects.header = input_msg->header;
 
@@ -250,6 +251,7 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
+  debugger_->publishProcessingTime();
 }
 
 void DetectionByTracker::divideUnderSegmentedObjects(

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -16,6 +16,7 @@
 #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_
 
 #include "pointcloud_preprocessor/filter.hpp"
+#include "pointcloud_preprocessor/transform_info.hpp"
 
 #include <vehicle_info_util/vehicle_info.hpp>
 
@@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   // classified point label
   // (0: not classified, 1: ground, 2: not ground, 3: follow previous point,
   //  4: unkown(currently not used), 5: virtual ground)
-  enum class PointLabel {
+  enum class PointLabel : uint16_t {
     INIT = 0,
     GROUND,
     NON_GROUND,
@@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
     VIRTUAL_GROUND,
     OUT_OF_RANGE
   };
-  struct PointRef
+
+  struct PointData
   {
-    float grid_size;    // radius of grid
-    uint16_t grid_id;   // id of grid in vertical
-    float radius;       // cylindrical coords on XY Plane
-    float theta;        // angle deg on XY plane
-    size_t radial_div;  // index of the radial division to which this point belongs to
+    float radius;  // cylindrical coords on XY Plane
     PointLabel point_state{PointLabel::INIT};
-
+    uint16_t grid_id;   // id of grid in vertical
     size_t orig_index;  // index of this point in the source pointcloud
-    pcl::PointXYZ * orig_point;
   };
-  using PointCloudRefVector = std::vector<PointRef>;
+  using PointCloudVector = std::vector<PointData>;
 
   struct GridCenter
   {
@@ -144,33 +141,57 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
 
     pcl::PointIndices getIndices() { return pcl_indices; }
     std::vector<float> getHeightList() { return height_list; }
+
+    pcl::PointIndices & getIndicesRef() { return pcl_indices; }
+    std::vector<float> & getHeightListRef() { return height_list; }
   };
 
   void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
 
+  // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes
+  // conform to new API
+  virtual void faster_filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
+    const pointcloud_preprocessor::TransformInfo & transform_info);
+
   tf2_ros::Buffer tf_buffer_{get_clock()};
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
 
+  int x_offset_;
+  int y_offset_;
+  int z_offset_;
+  int intensity_offset_;
+  bool offset_initialized_;
+
+  void set_field_offsets(const PointCloud2ConstPtr & input);
+
+  void get_point_from_global_offset(
+    const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset);
+
   const uint16_t gnd_grid_continual_thresh_ = 3;
   bool elevation_grid_mode_;
   float non_ground_height_threshold_;
   float grid_size_rad_;
+  float tan_grid_size_rad_;
   float grid_size_m_;
   float low_priority_region_x_;
   uint16_t gnd_grid_buffer_size_;
   float grid_mode_switch_grid_id_;
   float grid_mode_switch_angle_rad_;
   float virtual_lidar_z_;
   float detection_range_z_max_;
-  float center_pcl_shift_;                  // virtual center of pcl to center mass
-  float grid_mode_switch_radius_;           // non linear grid size switching distance
-  double global_slope_max_angle_rad_;       // radians
-  double local_slope_max_angle_rad_;        // radians
+  float center_pcl_shift_;             // virtual center of pcl to center mass
+  float grid_mode_switch_radius_;      // non linear grid size switching distance
+  double global_slope_max_angle_rad_;  // radians
+  double local_slope_max_angle_rad_;   // radians
+  double global_slope_max_ratio_;
+  double local_slope_max_ratio_;
   double radial_divider_angle_rad_;         // distance in rads between dividers
   double split_points_distance_tolerance_;  // distance in meters between concentric divisions
-  double                                    // minimum height threshold regardless the slope,
-    split_height_distance_;                 // useful for close points
+  double split_points_distance_tolerance_square_;
+  double                     // minimum height threshold regardless the slope,
+    split_height_distance_;  // useful for close points
   bool use_virtual_ground_point_;
   bool use_recheck_ground_cluster_;  // to enable recheck ground cluster
   size_t radial_dividers_num_;
@@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    */
 
   /*!
-   * Convert pcl::PointCloud to sorted PointCloudRefVector
+   * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector
    * @param[in] in_cloud Input Point Cloud to be organized in radial segments
    * @param[out] out_radial_ordered_points_manager Vector of Points Clouds,
    *     each element will contain the points ordered
    */
   void convertPointcloud(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
-    std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
+    const PointCloud2ConstPtr & in_cloud,
+    std::vector<PointCloudVector> & out_radial_ordered_points_manager);
   void convertPointcloudGridScan(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
-    std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
+    const PointCloud2ConstPtr & in_cloud,
+    std::vector<PointCloudVector> & out_radial_ordered_points_manager);
   /*!
    * Output ground center of front wheels as the virtual ground point
    * @param[out] point Virtual ground origin point
    */
   void calcVirtualGroundOrigin(pcl::PointXYZ & point);
 
+  float calcGridSize(const PointData & p);
+
   /*!
    * Classifies Points in the PointCloud as Ground and Not Ground
    * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud
@@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   void initializeFirstGndGrids(
     const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);
 
-  void checkContinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
-  void checkDiscontinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
-  void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
+  void checkContinuousGndGrid(
+    PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
+  void checkDiscontinuousGndGrid(
+    PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
+  void checkBreakGndGrid(
+    PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
   void classifyPointCloud(
-    std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
+    const PointCloud2ConstPtr & in_cloud_ptr,
+    std::vector<PointCloudVector> & in_radial_ordered_clouds,
     pcl::PointIndices & out_no_ground_indices);
   void classifyPointCloudGridScan(
-    std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
+    const PointCloud2ConstPtr & in_cloud_ptr,
+    std::vector<PointCloudVector> & in_radial_ordered_clouds,
     pcl::PointIndices & out_no_ground_indices);
   /*!
    * Re-classifies point of ground cluster based on their height
@@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    * and the other removed as indicated in the indices
    * @param in_cloud_ptr Input PointCloud to which the extraction will be performed
    * @param in_indices Indices of the points to be both removed and kept
-   * @param out_object_cloud_ptr Resulting PointCloud with the indices kept
+   * @param out_object_cloud Resulting PointCloud with the indices kept
    */
   void extractObjectPoints(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
-    pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
+    const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices,
+    PointCloud2 & out_object_cloud);
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;