Merge branch 'autowarefoundation:main' into feat/add_perception_objects_pointcloud_publisher

Author: lexavtanke

common/autoware_testing/package.xml

@@ -12,6 +12,8 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <buildtool_export_depend>ros_testing</buildtool_export_depend>
+
   <test_depend>ament_cmake_core</test_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -273,7 +273,7 @@ boost::optional<size_t> findNearestIndex(
 
   for (size_t i = 0; i < points.size(); ++i) {
     const auto squared_dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose);
-    if (squared_dist > max_squared_dist) {
+    if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) {
       continue;
     }
 
@@ -283,10 +283,6 @@ boost::optional<size_t> findNearestIndex(
       continue;
     }
 
-    if (squared_dist >= min_squared_dist) {
-      continue;
-    }
-
     min_squared_dist = squared_dist;
     min_idx = i;
     is_nearest_found = true;

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

@@ -47,6 +47,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `gnd_grid_buffer_size`            | uint16 | 4             | Number of grids using to estimate local ground slope,<br /> applied only for elevation_grid_mode                                                  |
 | `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                                                                                                                     |
 
 ## Assumptions / Known limits
 

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -170,6 +170,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   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_;
   VehicleInfo vehicle_info_;
 

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -54,6 +54,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
     split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2);
     split_height_distance_ = declare_parameter("split_height_distance", 0.2);
     use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true);
+    use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true);
     radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
     vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
 
@@ -356,7 +357,9 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
       // move to new grid
       if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
         // check if the prev grid have ground point cloud
-        recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
+        if (use_recheck_ground_cluster_) {
+          recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
+        }
         curr_gnd_grid.radius = ground_cluster.getAverageRadius();
         curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();
         curr_gnd_grid.max_height = ground_cluster.getMaxHeight();
@@ -611,7 +614,11 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter(
       get_logger(),
       "Setting use_virtual_ground_point to: " << std::boolalpha << use_virtual_ground_point_);
   }
-
+  if (get_param(p, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) {
+    RCLCPP_DEBUG_STREAM(
+      get_logger(),
+      "Setting use_recheck_ground_cluster to: " << std::boolalpha << use_recheck_ground_cluster_);
+  }
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   result.reason = "success";

perception/traffic_light_classifier/README.md

@@ -54,15 +54,19 @@ These colors and shapes are assigned to the message as follows:
 
 #### cnn_classifier
 
-| Name              | Type | Description                                       |
-| ----------------- | ---- | ------------------------------------------------- |
-| `model_file_path` | str  | path to the model file                            |
-| `label_file_path` | str  | path to the label file                            |
-| `precision`       | str  | TensorRT precision, `fp16` or `int8`              |
-| `input_c`         | str  | the channel size of an input image                |
-| `input_h`         | str  | the height of an input image                      |
-| `input_w`         | str  | the width of an input image                       |
-| `build_only`      | bool | shutdown node after TensorRT engine file is built |
+| Name              | Type   | Description                                       |
+| ----------------- | ------ | ------------------------------------------------- |
+| `model_file_path` | str    | path to the model file                            |
+| `label_file_path` | str    | path to the label file                            |
+| `precision`       | str    | TensorRT precision, `fp16` or `int8`              |
+| `input_c`         | str    | the channel size of an input image                |
+| `input_h`         | str    | the height of an input image                      |
+| `input_w`         | str    | the width of an input image                       |
+| `input_name`      | str    | the name of neural network's input layer          |
+| `output_name`     | str    | the name of neural network's output name          |
+| `mean`            | double | mean values for image normalization               |
+| `std`             | double | std values for image normalization                |
+| `build_only`      | bool   | shutdown node after TensorRT engine file is built |
 
 #### hsv_classifier
 

perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp

@@ -101,8 +101,8 @@ class CNNClassifier : public ClassifierInterface
   std::shared_ptr<Tn::TrtCommon> trt_;
   image_transport::Publisher image_pub_;
   std::vector<std::string> labels_;
-  std::vector<float> mean_{0.242, 0.193, 0.201};
-  std::vector<float> std_{1.0, 1.0, 1.0};
+  std::vector<double> mean_;
+  std::vector<double> std_;
   int input_c_;
   int input_h_;
   int input_w_;

perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml

@@ -19,7 +19,12 @@
     <param name="classifier_type" value="$(var classifier_type)"/>
     <param name="model_file_path" value="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_mobilenetv2.onnx"/>
     <param name="label_file_path" value="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt"/>
+    <param name="mean" value="[0.242, 0.193, 0.201]"/>
+    <param name="std" value="[1.0, 1.0, 1.0]"/>
     <param name="precision" value="fp16"/>
+    <param name="input_name" value="input_0"/>
+    <param name="output_name" value="output_0"/>
+    <param name="apply_softmax" value="true"/>
     <param name="input_c" value="3"/>
     <param name="input_h" value="224"/>
     <param name="input_w" value="224"/>

perception/traffic_light_classifier/src/cnn_classifier.cpp

@@ -39,8 +39,10 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr)
   input_c_ = node_ptr_->declare_parameter("input_c", 3);
   input_h_ = node_ptr_->declare_parameter("input_h", 224);
   input_w_ = node_ptr_->declare_parameter("input_w", 224);
-  auto input_name = node_ptr_->declare_parameter("input_name", "input_0");
-  auto output_name = node_ptr_->declare_parameter("output_name", "output_0");
+  mean_ = node_ptr_->declare_parameter("mean", std::vector<double>({0.242, 0.193, 0.201}));
+  std_ = node_ptr_->declare_parameter("std", std::vector<double>({1.0, 1.0, 1.0}));
+  std::string input_name = node_ptr_->declare_parameter("input_name", std::string("input_0"));
+  std::string output_name = node_ptr_->declare_parameter("output_name", std::string("output_0"));
   apply_softmax_ = node_ptr_->declare_parameter("apply_softmax", true);
 
   readLabelfile(label_file_path, labels_);

planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp

@@ -23,7 +23,6 @@
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <diagnostic_msgs/msg/key_value.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -46,8 +45,6 @@ namespace surround_obstacle_checker
 
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::Shape;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using motion_utils::VehicleStopChecker;
 using tier4_planning_msgs::msg::VelocityLimit;
 using tier4_planning_msgs::msg::VelocityLimitClearCommand;