mid-360 | 从环境搭建到前方扇形区域点云实时过滤与避障触发

1. 环境搭建：从零开始配置Mid-360开发环境

第一次接触Mid-360传感器时，最头疼的就是环境配置。记得当时我花了两天时间才把开发环境搭好，期间踩了不少坑。下面就把完整的配置流程分享给大家，帮你节省时间。

首先需要准备一台Ubuntu 18.04或20.04的电脑，建议使用物理机而不是虚拟机，因为点云处理对性能要求较高。我测试过，在虚拟机中运行经常会出现卡顿和数据丢失的情况。

第一步是安装Livox SDK2。打开终端，依次执行以下命令：

bash复制mkdir -p ~/livox_ws
cd ~/livox_ws
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2
mkdir build
cd build
cmake ..
make
sudo make install

这里有个小技巧：在make时可以加上-j4参数，这样能利用多核CPU加速编译。比如我的电脑是8核的，就会用make -j8。

接下来安装ROS驱动。Livox提供了ROS1和ROS2两个版本的驱动，这里以ROS1为例：

bash复制cd ~/livox_ws
mkdir -p src
cd src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
cd livox_ros_driver2
./build.sh ROS1

网络配置是另一个容易出问题的地方。Mid-360默认使用静态IP，需要将电脑的以太网口IP设置为192.168.1.5，子网掩码255.255.255.0。然后在MID360_config.json中修改lidar_configs的IP为192.168.1.1xx，其中xx是传感器SN号的后两位。

完成这些配置后，可以启动测试程序：

bash复制source ~/livox_ws/devel/setup.bash
roslaunch livox_ros_driver2 rviz_MID360.launch

如果一切正常，你应该能在RViz中看到实时的点云数据了。这里建议先检查点云的完整性和密度，确保传感器工作正常。

2. 理解Mid-360点云数据特性

Mid-360生成的原始点云是全向的，这意味着它会采集周围360度的环境信息。但在实际应用中，我们往往只需要关注特定方向的点云数据。以无人机避障为例，最需要关注的是正前方扇形区域的数据。

Mid-360的点云数据结构有几个重要特点：

1. 坐标系定义：X轴正方向为传感器正前方，Y轴为左侧，Z轴向上。这个定义和ROS的标准坐标系一致。

2. 数据格式：通过ROS发布的点云数据使用sensor_msgs/PointCloud2消息类型，每个点包含x、y、z三个坐标值。

3. 数据密度：在10米范围内，点云密度大约为每平方米2000个点，但随着距离增加密度会降低。

理解这些特性对后续的点云处理非常重要。比如在定义前方扇形区域时，我们需要基于这个坐标系进行计算。我刚开始时就犯过错误，把坐标系搞反了，导致提取的区域完全不对。

下面是一个简单的点云数据结构示例：

cpp复制struct Point3D {
    float x;  // 前向距离
    float y;  // 左右偏移
    float z;  // 高度
};

在实际应用中，我们还需要考虑点云的时间戳和坐标系转换。Mid-360的点云数据默认发布在livox_frame坐标系下，如果需要与其他传感器数据融合，可能需要进行坐标变换。

3. 前方扇形区域点云提取实战

现在来到核心部分：如何从全向点云中提取前方扇形区域的数据。我们需要定义一个水平±30度、距离0-2米的扇形区域，这个区域正好覆盖无人机飞行时需要监控的避障范围。

首先创建一个点云处理类：

cpp复制class FrontalSectorFilter {
public:
    FrontalSectorFilter(double max_distance = 2.0, double angle_range = 30.0) 
        : max_dist(max_distance), angle(angle_range) {}
    
    void process(const sensor_msgs::PointCloud2ConstPtr& input) {
        // 清空上一帧数据
        filtered_points.clear();
        
        // 创建迭代器遍历点云
        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*input, "x");
        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*input, "y");
        sensor_msgs::PointCloud2ConstIterator<float> iter_z(*input, "z");
        
        for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
            double distance = *iter_x;
            double lateral_angle = atan2(*iter_y, *iter_x) * 180.0 / M_PI;
            
            // 筛选条件
            if (distance > 0.1 && distance <= max_dist && 
                abs(lateral_angle) <= angle) {
                Point3D pt;
                pt.x = *iter_x;
                pt.y = *iter_y;
                pt.z = *iter_z;
                filtered_points.push_back(pt);
            }
        }
    }
    
    std::vector<Point3D> getFilteredPoints() const {
        return filtered_points;
    }

private:
    double max_dist;  // 最大检测距离(米)
    double angle;     // 水平角度范围(度)
    std::vector<Point3D> filtered_points;
};

这个类的工作原理是：

1. 遍历输入点云的每个点
2. 计算点到传感器的距离和水平角度
3. 只保留符合距离和角度条件的点

在实际测试中，我发现加入distance > 0.1的条件很有必要，可以过滤掉传感器自身产生的一些噪点。

为了验证提取效果，可以在RViz中可视化过滤后的点云：

cpp复制void publishFilteredCloud(ros::Publisher& pub, 
                        const std::vector<Point3D>& points,
                        const std::string& frame_id) {
    sensor_msgs::PointCloud2 output;
    // 设置消息头
    output.header.stamp = ros::Time::now();
    output.header.frame_id = frame_id;
    
    // 设置点云字段
    output.height = 1;
    output.width = points.size();
    output.is_dense = true;
    
    sensor_msgs::PointCloud2Modifier modifier(output);
    modifier.setPointCloud2Fields(3,
        "x", 1, sensor_msgs::PointField::FLOAT32,
        "y", 1, sensor_msgs::PointField::FLOAT32,
        "z", 1, sensor_msgs::PointField::FLOAT32);
    
    // 填充数据
    sensor_msgs::PointCloud2Iterator<float> out_x(output, "x");
    sensor_msgs::PointCloud2Iterator<float> out_y(output, "y");
    sensor_msgs::PointCloud2Iterator<float> out_z(output, "z");
    
    for (const auto& pt : points) {
        *out_x = pt.x; ++out_x;
        *out_y = pt.y; ++out_y;
        *out_z = pt.z; ++out_z;
    }
    
    pub.publish(output);
}

4. 避障触发逻辑设计与实现

有了前方扇形区域的点云数据后，接下来就是设计避障触发逻辑。这里我分享两种经过实测有效的方案：

4.1 基于点云密度的触发机制

这种方法的原理是：当区域内点云数量超过阈值时，认为检测到障碍物。实现代码如下：

cpp复制bool checkObstacleByDensity(const std::vector<Point3D>& points, 
                          int threshold = 50) {
    return points.size() > threshold;
}

这个方法的优点是实现简单，计算量小。但缺点是对远处障碍物不敏感，因为点云密度会随距离增加而降低。

4.2 基于最近距离的触发机制

更可靠的方法是计算区域内点云的最近距离：

cpp复制double findMinDistance(const std::vector<Point3D>& points) {
    if (points.empty()) return std::numeric_limits<double>::max();
    
    double min_dist = points[0].x;  // x轴方向就是前向距离
    for (const auto& pt : points) {
        double dist = pt.x;  // 也可以计算欧式距离sqrt(x²+y²+z²)
        if (dist < min_dist) {
            min_dist = dist;
        }
    }
    return min_dist;
}

bool checkObstacleByDistance(const std::vector<Point3D>& points,
                           double safe_distance = 1.0) {
    return findMinDistance(points) < safe_distance;
}

在实际项目中，我建议结合两种方法：先用密度检测快速判断是否有障碍物，再用距离检测确认危险程度。这样可以平衡响应速度和准确性。

最后，将检测结果发送给飞控系统：

cpp复制void sendHaltCommand(ros::Publisher& cmd_pub, bool should_stop) {
    std_msgs::Bool msg;
    msg.data = should_stop;
    cmd_pub.publish(msg);
}

5. 性能优化与实战技巧

在实际部署中，我发现有几个关键点会显著影响系统性能：

1. 点云降采样：原始点云数据量很大，可以对输入点云先进行降采样处理。VoxelGrid滤波是个不错的选择：

cpp复制pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(input_cloud);
voxel.setLeafSize(0.05f, 0.05f, 0.05f);  // 5cm的立方体格子
voxel.filter(*filtered_cloud);

2. 多线程处理：点云处理比较耗时，建议使用单独的线程进行处理，避免阻塞主线程。

3. 参数调优：扇形区域的角度和距离范围需要根据实际场景调整。在狭窄环境中，角度可以小一些；在开阔环境则可以大一些。

4. 滤波处理：加入统计离群值移除可以过滤噪点：

cpp复制pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*filtered_cloud);

5. 实时监控：建议在RViz中同时显示原始点云和过滤后的点云，方便调试。可以给不同区域的点云设置不同颜色，比如危险区域用红色，安全区域用绿色。

经过这些优化后，在我的测试中，整个处理流程能在10ms内完成，完全可以满足实时避障的需求。