当前位置:   article > 正文

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

mid360 仿真数据集

本文将使用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
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

完成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
  • 1
  • 2
  • 3

完成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 
  • 1

在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;
};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62

在主函数中接收/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");
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

在rviz中调整显示的话题,可观察到仅有传感器前方区域的点云数据。
后续可对点云数目进行判断,大于某一个值时执行悬停或给定速度为0以完成简单的避障

TODO

解析特定方向点云数据,判断障碍物条件成立后,发送标志位至控制器进入悬停模式

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/weixin_40725706/article/detail/481477
推荐阅读
相关标签
  

闽ICP备14008679号