mid-360|环境配置及传感器特定方向点云数据提取

yangjh542426

已于 2024-01-31 11:58:10 修改

阅读量1.1k

点赞数 8

分类专栏： px4 ros ubuntu 文章标签： ubuntu 无人机

于 2024-01-25 20:13:10 首次发布

本文链接：https://blog.csdn.net/m0_46182398/article/details/135852755

版权

px4 同时被 3 个专栏收录

14 篇文章 4 订阅

订阅专栏

ubuntu

12 篇文章 1 订阅

订阅专栏

ros

9 篇文章 0 订阅

订阅专栏

本文将使用mid360实现简单的识别前方有障碍物时无人机悬停功能

环境配置

新建文件夹用于存储SDK以及ROS包

git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2
mkdir build
cd build
cmake ..
make
sudo make install

完成sdk的安装
根目录下

git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
cd src/livox_ros_driver2/
./build ROS1

完成ROS运行环境安装
修改本地以太网口ip地址为192.168.1.5
修改MID360_config.json文件中lidar_configs的ip为192.168.1.1xx其中xx为mid360sn号的后两位

执行

roslaunch livox_ros_driver2 rviz_MID360.launch

在rviz界面中可以观察到点云数据即成功

获取特定方向点云数据

构造结构体用于存储点云数据

class PointCloud {
public:
    // 构造函数
    PointCloud() {}

    // 添加点的方法
    void addPoint(double x, double y, double z) {
        points.push_back({x, y, z});
    }

    // 获取点的数量
    size_t size() const {
        return points.size();
    }

    // 获取所有点的坐标
    std::vector<std::vector<double>> getPoints() const {
        return points;
    }

    // 打印所有点的坐标
    void printPoints() const {
        std::cout << "Points in the PointCloud:" << std::endl;
        int index = 1;
        for (const auto& point : points) {
            std::cout << "x: " << point[0] << "  y: " << point[1] << "  z: " << point[2] << "index:"<<index++<<std::endl;
        }
    }

    // 发布点云数据
    void publishPointCloud(ros::Publisher& pub,std::string frame_id) {
        // 创建 PointCloud2 消息
        sensor_msgs::PointCloud2 msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = frame_id;
        msg.height = 1;
        msg.width = points.size();
        msg.is_dense = true;

        // 设置字段
        sensor_msgs::PointCloud2Modifier modifier(msg);
        modifier.setPointCloud2FieldsByString(1, "xyz");
        sensor_msgs::PointCloud2Iterator<float> iter_x(msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");

        // 将点的坐标添加到消息中
        for (const auto& point : points) {
            *iter_x = point[0];
            *iter_y = point[1];
            *iter_z = point[2];
            ++iter_x; ++iter_y; ++iter_z;
        }

        // 发布消息
        publisher.publish(msg);
    }

private:
    // 存储点的坐标
    std::vector<std::vector<double>> points;
};

在主函数中接收/livox/lidar话题信息，并在回调函数中保存传感器前方112范围内的点云数据用于前方避障

void callbackPointCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud) {
    PointCloud pointCloud;
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud, "z");

    // Simulate some processing delay (adjust as needed)
    std::this_thread::sleep_for(std::chrono::seconds(1));
    int index=1;

    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
        if(*iter_x<2&&*iter_x>0.001&&std::abs(*iter_y)<0.5&&std::abs(*iter_z)<0.5)
            pointCloud.addPoint(*iter_x, *iter_y, *iter_z);
        // ROS_INFO("x: %.3f  y: %.3f  z: %.3f  index:%d", *iter_x, *iter_y, *iter_z, index++);
    }
    pointCloud.printPoints();

    // ros::Publisher 
    // 发布 PointCloud2 消息
    pointCloud.publishPointCloud(publisher, "livox_frame");
}