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),])