autoware.universe源码略读(3.7)--perception：elevation_map_loader/euclidean_cluster

elevation_map_loader

在上一篇文章有提到compare_map_segmentation这个滤波的操作，这个模块就是生成海拔图用的。这个也是很标准的一个节点类（在网上又看了一点ROS2的知识，似乎叫组件更合理一些？） 这里我们可以看到，使用到了grid_map_pcl_loader_这样一个变量

  grid_map_pcl_loader_ = pcl::make_shared<grid_map::GridMapPclLoader>(grid_map_logger);
  grid_map_pcl_loader_->loadParameters(param_file_path);

然后在话题的订阅与发布中，分别订阅了三个话题：map_hash，pointcloud_map以及vector_map

  sub_map_hash_ = create_subscription<tier4_external_api_msgs::msg::MapHash>(
    "/api/autoware/get/map/info/hash", durable_qos,
    std::bind(&ElevationMapLoaderNode::onMapHash, this, _1));
  sub_pointcloud_map_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
    "input/pointcloud_map", durable_qos,
    std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1));
  sub_vector_map_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
    "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1));

在三个订阅话题的回调函数里都是根据消息进行了一些准备工作，最重要的变量就是data_manager_，三个回调函数分别设置了这个变量的一些对象

  // onMapHash
  data_manager_.elevation_map_path_ = std::make_unique<std::filesystem::path>(
    std::filesystem::path(elevation_map_directory_) / elevation_map_hash);
  // onPointcloudMap
  data_manager_.map_pcl_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
  // onVectorMap
  data_manager_.lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
  lanelet::utils::conversion::fromBinMsg(*vector_map, data_manager_.lanelet_map_ptr_);
  const lanelet::ConstLanelets all_lanelets =
    lanelet::utils::query::laneletLayer(data_manager_.lanelet_map_ptr_);
  lane_filter_.road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets);

当data_manager_.isInitialized()通过的时候，会调用到publish函数，这里并不是一定要有已经存在的海拔图的，可以看到在函数最前面先判断海拔地图是否存在

1. 如果不存在的话，就来生成一个

  if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) {
    RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
    createElevationMap();

生成海拔地图是分成了是否使用车道线滤波两种方式，如果不使用就直接为grid_map_pcl_loader_设置输入点云了，使用的话就先对点云进行一下滤波的操作，然后把滤波过的点云作为输入点云

    const auto convex_hull = getConvexHull(data_manager_.map_pcl_ptr_);
    lanelet::ConstLanelets intersected_lanelets =
      getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr lane_filtered_map_pcl_ptr =
      getLaneFilteredPointCloud(intersected_lanelets, data_manager_.map_pcl_ptr_);
    grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr);

具体生成的部分createElevationMapFromPointcloud，应该是调用grid_map库来实现的

void ElevationMapLoaderNode::createElevationMapFromPointcloud()
{
  const auto start = std::chrono::high_resolution_clock::now();
  grid_map_pcl_loader_->preProcessInputCloud();
  grid_map_pcl_loader_->initializeGridMapGeometryFromInputCloud();
  grid_map_pcl_loader_->addLayerFromInputCloud(layer_name_);
  grid_map::grid_map_pcl::printTimeElapsedToRosInfoStream(
    start, "Finish creating elevation map. Total time: ", this->get_logger());
}

接下来还有一步就是inpaintElevationMap，事实上经过之前的操作，我们已经得到了elevation_map_，但现在的这个海拔图可能有很多孔雀，也并不连贯，所以这个函数的作用就是让海拔图更连续一些，具体是直接用cv::inpaint(original_image, mask, filled_image, radius_in_pixels, cv::INPAINT_NS);来对整个海拔图进行修补，然后把图像再转回elevation_map_

2. 如果海拔图存在的话，会尝试加载海拔图，加载成功就直接用已经有的海拔图了，加载失败的话就调用createElevationMap()函数去生成，这里也是用到了grid_map库

  } else if (info.st_mode & S_IFDIR) {
    RCLCPP_INFO(
      this->get_logger(), "Load elevation map from: %s",
      data_manager_.elevation_map_path_->c_str());

    // Check if bag can be loaded
    bool is_bag_loaded = false;
    try {
      is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag(
        *data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
    } catch (rosbag2_storage_plugins::SqliteException & e) {
      is_bag_loaded = false;
    }
    if (!is_bag_loaded) {
      // Delete directory including elevation map if bag is broken
      RCLCPP_ERROR(
        this->get_logger(), "Try to loading bag, but bag is broken. Remove %s",
        data_manager_.elevation_map_path_->c_str());
      std::filesystem::remove_all(data_manager_.elevation_map_path_->c_str());
      // Create elevation map from pointcloud map if bag is broken
      RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
      createElevationMap();
    }
  }

最后就是对海拔地图，以及海拔地图对应的点云进行发布了

  pub_elevation_map_->publish(std::move(msg));  // 这里用到move是因为后边就不用msg了，所以能避免不必要的copy

  if (use_elevation_map_cloud_publisher_) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr elevation_map_cloud_ptr =
      createPointcloudFromElevationMap();
    sensor_msgs::msg::PointCloud2 elevation_map_cloud_msg;
    pcl::toROSMsg(*elevation_map_cloud_ptr, elevation_map_cloud_msg);
    pub_elevation_map_cloud_->publish(elevation_map_cloud_msg);
  }

当然，这个包也是最新的版本会比galatic更复杂严谨一些的，不过这里没看最新版本的是什么样的

euclidean_cluster

欧式聚类，这个在上边刚刚用到，简单来说就是把点云聚类成更小的部分以进行物体的分类的步骤。这里是分了两种方法，一种是欧式聚类，也就是euclidean_cluster，另一种是voxel_grid_based_euclidean_cluster，看起来是根据体素网格进行分类的一种办法。

euclidean_cluster

这里的构造函数输入了几个参数，分别是是否使用高度信息，聚类最少点数，最多点数以及点之间的最大距离阈值，然后这里整个类EuclideanCluster是继承自EuclideanClusterInterface的，所以有几个成员变量也是继承过来的。然后主要的聚类操作的函数就在clustrer，这里我们可以看到，其实也是用到了KD树的

  // create tree
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(pointcloud_ptr);

然后直接用PCL库中的聚类函数实现

  // clustering
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> pcl_euclidean_cluster;
  pcl_euclidean_cluster.setClusterTolerance(tolerance_);
  pcl_euclidean_cluster.setMinClusterSize(min_cluster_size_);
  pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_);
  pcl_euclidean_cluster.setSearchMethod(tree);
  pcl_euclidean_cluster.setInputCloud(pointcloud_ptr);
  pcl_euclidean_cluster.extract(cluster_indices);

然后就是输出了

voxel_grid_based_euclidean_cluster

VoxelGridBasedEuclideanCluster这个类也是继承自EuclideanClusterInterface的，所以这里的构造函数和EuclideanCluster一样，也是对接口类的几个成员变量进行了设置，这里还有两个成员变量，一个是 voxel_leaf_size_(voxel_leaf_size)，这个代表的应该就是体素网格的大小；另一个是min_points_number_per_voxel_，这里对应的应该是一个体素网格里最少的点数。然后这个方法的核心操作也是在clustre函数中，这里一上来也是设置了一些参数

  pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0);
  voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
  voxel_grid_.setInputCloud(pointcloud);
  voxel_grid_.setSaveLeafLayout(true);
  voxel_grid_.filter(*voxel_map_ptr);

我们可以看到划分网格的时候，z方向其实是不划分的，所以这样网格就可以看成是一个二维的网格了。然后这里的filter我的理解应该就是把点云按照体素网格划分开来了。之后也是用到了KD树，值得注意的是，因为这里其实是不考虑Z轴信息的，所以点云也是被转成了2D点云

  // voxel is pressed 2d
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_2d_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  for (const auto & point : voxel_map_ptr->points) {
    pcl::PointXYZ point2d;
    point2d.x = point.x;
    point2d.y = point.y;
    point2d.z = 0.0;
    pointcloud_2d_ptr->push_back(point2d);
  }

  // create tree
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(pointcloud_2d_ptr);

然后聚类也是一样，直接用PCL的pcl::EuclideanClusterExtraction

  // clustering
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> pcl_euclidean_cluster;
  pcl_euclidean_cluster.setClusterTolerance(tolerance_);
  pcl_euclidean_cluster.setMinClusterSize(1);
  pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_);
  pcl_euclidean_cluster.setSearchMethod(tree);
  pcl_euclidean_cluster.setInputCloud(pointcloud_2d_ptr);
  pcl_euclidean_cluster.extract(cluster_indices);

将点云中的点根据其在体素网格中的位置进行聚类，结果存储在temporary_clusters中。这样，就可以通过体素网格索引快速查找点所在的聚类。

  // create map to search cluster index from voxel grid index
  std::unordered_map</* voxel grid index */ int, /* cluster index */ int> map;
  for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) {
    const auto & cluster = cluster_indices.at(cluster_idx);
    for (const auto & point_idx : cluster.indices) {
      map[point_idx] = cluster_idx;
    }
  }

  // create vector of point cloud cluster. vector index is voxel grid index.
  std::vector<pcl::PointCloud<pcl::PointXYZ>> temporary_clusters;  // no check about cluster size
  temporary_clusters.resize(cluster_indices.size());
  for (const auto & point : pointcloud->points) {
    const int index =
      voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
    if (map.find(index) != map.end()) {
      temporary_clusters.at(map[index]).points.push_back(point);
    }
  }

最后也是把这些作为结果输出就好了

节点类

这两个的节点类是差不多的，构造函数加载参数，实例化聚类的类，然后订阅话题是点云

  using std::placeholders::_1;
  pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
    "input", rclcpp::SensorDataQoS().keep_last(1),
    std::bind(&VoxelGridBasedEuclideanClusterNode::onPointCloud, this, _1));

然后在回调函数中，执行了聚类操作以及消息类型的各种转换

  // convert ros to pcl
  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);

  // clustering
  std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;
  cluster_->cluster(raw_pointcloud_ptr, clusters);

  // build output msg
  tier4_perception_msgs::msg::DetectedObjectsWithFeature output;
  convertPointCloudClusters2Msg(input_msg->header, clusters, output);
  cluster_pub_->publish(output);

launch文件

这里的launch文件是使用了launch+py文件的这种形式，在launch文件里主要是加载了程序要使用到的各种参数，然后在python文件中是具体的启动了节点，在python文件中，可以看到还加载了几个组件

    # set voxel grid filter as a component
    voxel_grid_filter_component = ComposableNode(
        package="pointcloud_preprocessor",
        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
        name=AnonName("voxel_grid_filter"),
        remappings=[
            ("input", LaunchConfiguration("input_pointcloud")),
            ("output", "voxel_grid_filtered/pointcloud"),
        ],
        parameters=[load_composable_node_param("voxel_grid_param_path")],
    )

    # set compare map filter as a component
    compare_map_filter_component = ComposableNode(
        package="compare_map_segmentation",
        plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
        name=AnonName("compare_map_filter"),
        remappings=[
            ("input", "voxel_grid_filtered/pointcloud"),
            ("map", LaunchConfiguration("input_map")),
            ("output", "compare_map_filtered/pointcloud"),
        ],
    )

然后再具体一点，我们可以看到话题的名字在这里是有remapping操作的，所以代码里都是input和output

    use_map_euclidean_cluster_component = ComposableNode(
        package=pkg,
        plugin="euclidean_cluster::EuclideanClusterNode",
        name=AnonName("euclidean_cluster"),
        remappings=[
            ("input", "compare_map_filtered/pointcloud"),
            ("output", LaunchConfiguration("output_clusters")),
        ],
        parameters=[load_composable_node_param("euclidean_param_path")],
    )

然后其实具体的启动函数是在generate_launch_description中的，里面会调用launch_setup这个函数

def generate_launch_description():
    def add_launch_arg(name: str, default_value=None):
        return DeclareLaunchArgument(name, default_value=default_value)

    return launch.LaunchDescription(
        [
            add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"),
            add_launch_arg("input_map", "/map/pointcloud_map"),
            add_launch_arg("output_clusters", "clusters"),
            add_launch_arg("use_pointcloud_map", "false"),
            add_launch_arg(
                "voxel_grid_param_path",
                [FindPackageShare("euclidean_cluster"), "/config/voxel_grid.param.yaml"],
            ),
            add_launch_arg(
                "euclidean_param_path",
                [FindPackageShare("euclidean_cluster"), "/config/euclidean_cluster.param.yaml"],
            ),
            OpaqueFunction(function=launch_setup),
        ]
    )