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_，三個回調函數分別設置了這個變量的一些對象

  // onMapHashdata_manager_.elevation_map_path_ = std::make_unique<std::filesystem::path>(std::filesystem::path(elevation_map_directory_) / elevation_map_hash);// onPointcloudMapdata_manager_.map_pcl_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);// onVectorMapdata_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 loadedbool 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 brokenRCLCPP_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 brokenRCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");createElevationMap();}}

最后就是對海拔地圖，以及海拔地圖對應的點云進行發布了

  pub_elevation_map_->publish(std::move(msg));  // 這里用到move是因為后邊就不用msg了，所以能避免不必要的copyif (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 treepcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);tree->setInputCloud(pointcloud_ptr);

然后直接用PCL庫中的聚類函數實現

  // clusteringstd::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 2dpcl::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 treepcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);tree->setInputCloud(pointcloud_2d_ptr);

然后聚類也是一樣，直接用PCL的pcl::EuclideanClusterExtraction

  // clusteringstd::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 indexstd::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 sizetemporary_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 pclpcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);// clusteringstd::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;cluster_->cluster(raw_pointcloud_ptr, clusters);// build output msgtier4_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 componentvoxel_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 componentcompare_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),])