Merge branch 'main' into feat/fault-injection/change-for-diagnostic-g…

…raph-aggregator

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

@@ -0,0 +1,66 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace tier4_autoware_utils
+{
+
+template <typename T>
+class InterProcessPollingSubscriber
+{
+private:
+  typename rclcpp::Subscription<T>::SharedPtr subscriber_;
+  std::optional<T> data_;
+
+public:
+  explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
+  {
+    auto noexec_callback_group =
+      node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    auto noexec_subscription_options = rclcpp::SubscriptionOptions();
+    noexec_subscription_options.callback_group = noexec_callback_group;
+
+    subscriber_ = node->create_subscription<T>(
+      topic_name, rclcpp::QoS{1},
+      [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
+      noexec_subscription_options);
+  };
+  bool updateLatestData()
+  {
+    rclcpp::MessageInfo message_info;
+    T tmp;
+    // The queue size (QoS) must be 1 to get the last message data.
+    if (subscriber_->take(tmp, message_info)) {
+      data_ = tmp;
+    }
+    return data_.has_value();
+  };
+  const T & getData() const
+  {
+    if (!data_.has_value()) {
+      throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
+    }
+    return data_.value();
+  };
+};
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_

evaluator/perception_online_evaluator/README.md

@@ -11,6 +11,7 @@ This module allows for the evaluation of how accurately perception results are g
 - Calculates lateral deviation between the predicted path and the actual traveled trajectory.
 - Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition.
 - Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition.
+- Calculates yaw rate based on the yaw of the object received in the previous cycle to evaluate the stability of the yaw rate recognition.
 
 ## Inputs / Outputs
 

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

@@ -381,12 +381,17 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas
       if (time_diff < 0.01) {
         continue;
       }
-      const auto current_yaw =
+      const double current_yaw =
         tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation);
-      const auto previous_yaw =
+      const double previous_yaw =
         tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation);
-      const auto yaw_rate =
-        std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw) / time_diff);
+      const double yaw_diff =
+        std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw));
+      // if yaw_diff is close to PI, reversal of orientation is likely occurring, so ignore it
+      if (std::abs(M_PI - yaw_diff) < 0.1) {
+        continue;
+      }
+      const auto yaw_rate = yaw_diff / time_diff;
       stat.add(yaw_rate);
     }
     metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat;

evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

@@ -214,6 +214,7 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame
   auto & p = parameters_;
 
   updateParam<size_t>(parameters, "smoothing_window_size", p->smoothing_window_size);
+  updateParam<double>(parameters, "stopped_velocity_threshold", p->stopped_velocity_threshold);
 
   // update metrics
   {

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -327,15 +327,19 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     // objects_pub_->publish(*input_objects);
     return;
   }
+  bool validation_is_ready = true;
   if (!validator_->setKdtreeInputCloud(input_obstacle_pointcloud)) {
-    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud");
-    return;
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5,
+      "obstacle pointcloud is empty! Can not validate objects.");
+    validation_is_ready = false;
   }
 
   for (size_t i = 0; i < transformed_objects.objects.size(); ++i) {
     const auto & transformed_object = transformed_objects.objects.at(i);
     const auto & object = input_objects->objects.at(i);
-    const auto validated = validator_->validate_object(transformed_object);
+    const auto validated =
+      validation_is_ready ? validator_->validate_object(transformed_object) : false;
     if (debugger_) {
       debugger_->addNeighborPointcloud(validator_->getDebugNeighborPointCloud());
       debugger_->addPointcloudWithinPolygon(validator_->getDebugPointCloudWithinObject());

perception/ground_segmentation/config/ground_segmentation.param.yaml

@@ -33,3 +33,4 @@
         center_pcl_shift: 0.0
         radial_divider_angle_deg: 1.0
         use_recheck_ground_cluster: true
+        use_lowest_point: true

perception/ground_segmentation/config/scan_ground_filter.param.yaml

@@ -15,3 +15,4 @@
     center_pcl_shift: 0.0
     radial_divider_angle_deg: 1.0
     use_recheck_ground_cluster: true
+    use_lowest_point: true

perception/ground_segmentation/docs/scan-ground-filter.md

@@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `low_priority_region_x`           | float  | -20.0         | The non-zero x threshold in back side from which small objects detection is low priority [m]                                                                                                                                                                                                                                                                     |
 | `elevation_grid_mode`             | bool   | true          | Elevation grid scan mode option                                                                                                                                                                                                                                                                                                                                  |
 | `use_recheck_ground_cluster`      | bool   | true          | Enable recheck ground cluster                                                                                                                                                                                                                                                                                                                                    |
+| `use_lowest_point`                | bool   | true          | to select lowest point for reference in recheck ground cluster, otherwise select middle point                                                                                                                                                                                                                                                                    |
 
 ## Assumptions / Known limits
 

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -194,6 +194,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
     split_height_distance_;  // useful for close points
   bool use_virtual_ground_point_;
   bool use_recheck_ground_cluster_;  // to enable recheck ground cluster
+  bool use_lowest_point_;  // to select lowest point for reference in recheck ground cluster,
+                           // otherwise select middle point
   size_t radial_dividers_num_;
   VehicleInfo vehicle_info_;
 
@@ -258,7 +260,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    * @param non_ground_indices Output non-ground PointCloud indices
    */
   void recheckGroundCluster(
-    PointsCentroid & gnd_cluster, const float non_ground_threshold,
+    PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
     pcl::PointIndices & non_ground_indices);
   /*!
    * Returns the resulting complementary PointCloud, one with the points kept

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
     split_height_distance_ = declare_parameter<float>("split_height_distance");
     use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
     use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
+    use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
     radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
     vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
 
@@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid(
 }
 
 void ScanGroundFilterComponent::recheckGroundCluster(
-  PointsCentroid & gnd_cluster, const float non_ground_threshold,
+  PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
   pcl::PointIndices & non_ground_indices)
 {
-  const float min_gnd_height = gnd_cluster.getMinHeight();
+  float reference_height =
+    use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight();
   const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef();
   const std::vector<float> & height_list = gnd_cluster.getHeightListRef();
   for (size_t i = 0; i < height_list.size(); ++i) {
-    if (height_list.at(i) >= min_gnd_height + non_ground_threshold) {
+    if (height_list.at(i) >= reference_height + non_ground_threshold) {
       non_ground_indices.indices.push_back(gnd_indices.indices.at(i));
     }
   }
@@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
       if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
         // check if the prev grid have ground point cloud
         if (use_recheck_ground_cluster_) {
-          recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
+          recheckGroundCluster(
+            ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices);
         }
         curr_gnd_grid.radius = ground_cluster.getAverageRadius();
         curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();

perception/ground_segmentation/test/test_scan_ground_filter.cpp

@@ -83,6 +83,7 @@ class ScanGroundFilterTest : public ::testing::Test
       rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_));
     parameters.emplace_back(
       rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_));
+    parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_));
 
     options.parameter_overrides(parameters);
 
@@ -157,6 +158,7 @@ class ScanGroundFilterTest : public ::testing::Test
     center_pcl_shift_ = params["center_pcl_shift"].as<float>();
     radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as<float>();
     use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as<bool>();
+    use_lowest_point_ = params["use_lowest_point"].as<bool>();
   }
 
   float global_slope_max_angle_deg_ = 0.0;
@@ -174,6 +176,7 @@ class ScanGroundFilterTest : public ::testing::Test
   float center_pcl_shift_;
   float radial_divider_angle_deg_;
   bool use_recheck_ground_cluster_;
+  bool use_lowest_point_;
 };
 
 TEST_F(ScanGroundFilterTest, TestCase1)

perception/multi_object_tracker/CMakeLists.txt

@@ -47,12 +47,14 @@ ament_auto_add_library(multi_object_tracker_node SHARED
 
 target_link_libraries(multi_object_tracker_node
   Eigen3::Eigen
-  glog::glog
 )
 
-rclcpp_components_register_node(multi_object_tracker_node
-  PLUGIN "MultiObjectTracker"
-  EXECUTABLE multi_object_tracker
+ament_auto_add_executable(${PROJECT_NAME}
+  src/multi_object_tracker_node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  multi_object_tracker_node glog
 )
 
 ament_auto_package(INSTALL_TO_SHARE

perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp

@@ -41,7 +41,10 @@ class TrackerDebugger
     const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
   void startMeasurementTime(
     const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp);
+  void endMeasurementTime(const rclcpp::Time & now);
+  void startPublishTime(const rclcpp::Time & now);
   void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time);
+
   void setupDiagnostics();
   void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
   struct DEBUG_SETTINGS
@@ -56,12 +59,15 @@ class TrackerDebugger
 
 private:
   void loadParameters();
+  bool is_initialized_ = false;
   rclcpp::Node & node_;
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
     debug_tentative_objects_pub_;
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
   rclcpp::Time last_input_stamp_;
   rclcpp::Time stamp_process_start_;
+  rclcpp::Time stamp_process_end_;
+  rclcpp::Time stamp_publish_start_;
   rclcpp::Time stamp_publish_output_;
 };
 

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -58,7 +58,11 @@ class MultiObjectTracker : public rclcpp::Node
     tracked_objects_pub_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     detected_object_sub_;
-  rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
+
+  // publish timer
+  rclcpp::TimerBase::SharedPtr publish_timer_;
+  rclcpp::Time last_published_time_;
+  double publisher_period_;
 
   // debugger class
   std::unique_ptr<TrackerDebugger> debugger_;
@@ -79,14 +83,14 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<DataAssociation> data_association_;
 
   void checkTrackerLifeCycle(
-    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform);
+    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
   void sanitizeTracker(
     std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
   std::shared_ptr<Tracker> createNewTracker(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) const;
 
+  void checkAndPublish(const rclcpp::Time & time);
   void publish(const rclcpp::Time & time) const;
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
 };