德国大陆毫米波雷达(ARS548)ROS驱动更改

---

前言

ARS548的原始ROS驱动提供detection、object两种类型信息，这些信息均是自定义msg，不能直接通过topic发布sensor_msgs::PointCloud2和sensor_msgs::PointCloud信息。下面讲介绍detection。

---

Detecion

detection是提供的点云信息，而object是提供雷达检测到的具体物体信息。 下面是detection具体的相关信息。

我想要得到XYZ、RCS、Doppler、Probability，下面是转换函数代码。

void detectionReceive(const ars548_msg::DetectionList &msg)
{
    ROS_INFO("cl2run");
    uint size = msg.detection_array.size();

    // sensor_msgs::convertPointCloudToPointCloud2();

    static sensor_msgs::PointCloud2 cloud_msg;
    static sensor_msgs::PointCloud cloud_msg1;

    cloud_msg.header.frame_id = "/radar";
    cloud_msg.header.stamp = msg.detection_array[0].header.stamp;

    cloud_msg1.header.frame_id = "/radar";
    cloud_msg1.header.stamp = msg.detection_array[0].header.stamp;


    // 设置 PointCloud 的点和通道
    cloud_msg1.points.resize(size);
    cloud_msg1.channels.resize(3); // 我们有三个通道：probability 、 doppler、rcs
    cloud_msg1.channels[0].name = "probability";
    cloud_msg1.channels[0].values.resize(size);
    cloud_msg1.channels[1].name = "doppler";
    cloud_msg1.channels[1].values.resize(size);
    cloud_msg1.channels[2].name = "rcs";
    cloud_msg1.channels[2].values.resize(size);
    

    // 定义PointCloud2消息的字段
    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
    // modifier.setPointCloud2FieldsByString(3, "xyz");
    // modifier.setPointCloud2FieldsByString(4, "x", "y", "z", "intensity", "velocity");

    // 设置PointCloud2消息的宽度和高度
    cloud_msg.width = size;
    cloud_msg.height = 1;

    modifier.setPointCloud2Fields(6, "x", 1, sensor_msgs::PointField::FLOAT32,
                                  "y", 1, sensor_msgs::PointField::FLOAT32,
                                  "z", 1, sensor_msgs::PointField::FLOAT32,
                                  "probability", 1, sensor_msgs::PointField::FLOAT32,
                                  "doppler", 1, sensor_msgs::PointField::FLOAT32,
                                  "rcs", 1, sensor_msgs::PointField::FLOAT32);

    // for (size_t i = 0; i < size; ++i)
    // {
    //     // 计算字段在点云数据中的偏移位置
    //     uint8_t *point_data = &cloud_msg.data[i * cloud_msg.point_step];
    //     *(_Float32 *)(point_data + cloud_msg.fields[0].offset) = msg.detection_array[i].f_x;
    //     *(_Float32 *)(point_data + cloud_msg.fields[1].offset) = msg.detection_array[i].f_y;
    //     *(_Float32 *)(point_data + cloud_msg.fields[2].offset) = msg.detection_array[i].f_z;
    //     *(_Float32 *)(point_data + cloud_msg.fields[3].offset) = msg.detection_array[i].s_RCS;
    //     *(_Float32 *)(point_data + cloud_msg.fields[4].offset) = 0;
    // }

    // 获取字段的偏移量
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
    sensor_msgs::PointCloud2Iterator<float> iter_probability(cloud_msg, "probability");
    sensor_msgs::PointCloud2Iterator<float> iter_doppler(cloud_msg, "doppler");
    sensor_msgs::PointCloud2Iterator<float> iter_rcs(cloud_msg, "rcs");


    // 填充PointCloud2消息的数据
    for (uint i = 0; i < size; i++)
    {
        *iter_x = msg.detection_array[i].f_x;
        *iter_y = msg.detection_array[i].f_y;
        *iter_z = msg.detection_array[i].f_z;
        *iter_probability =msg.detection_array[i].u_PositivePredictiveValue;
        *iter_doppler =msg.detection_array[i].f_RangeRate;
        *iter_rcs =msg.detection_array[i].s_RCS;


        ++iter_x;
        ++iter_y;
        ++iter_z;
        ++iter_probability;
        ++iter_doppler;
        ++iter_rcs;

        cloud_msg1.points[i].x = msg.detection_array[i].f_x;
        cloud_msg1.points[i].y = msg.detection_array[i].f_y; 
        cloud_msg1.points[i].z = msg.detection_array[i].f_z;
        cloud_msg1.channels[0].values[i] = msg.detection_array[i].u_PositivePredictiveValue;
        cloud_msg1.channels[1].values[i] = msg.detection_array[i].f_RangeRate;
        cloud_msg1.channels[2].values[i] = msg.detection_array[i].s_RCS;


    }

    // 发布点云消息
    point_cloud2_pub.publish(cloud_msg);
    point_cloud_pub.publish(cloud_msg1);


    // detections_header_pub2.publish(cloud_msg.header);
}