【cartographer without ros】三、雷达scan数据转换

上一节介绍使用cartographer的思路和大纲，其中第一步是先处理各类原始数据转换成cartographer数据类型。

本节就开始介绍将最常用的激光雷达LaserScan传感数据转换为cartographer中的TimedPointCloudData数据。还有cartographer中的TimedPointCloudData数据和Ros的sensor_msgs::LaserScan数据相互转换，方便引入Rosbag进行储存和读取。

目录

1：TimedPointCloudData数据类型

2：LaserScan传感数据转换cartographer数据

3：LaserScan传感数据转换Ros数据

4：cartographer数据转换Ros数据

5：Ros数据转换cartographer数据

---

1：TimedPointCloudData数据类型

【timed_point_cloud_data.h】中查看数据结构：

//时间
common::Time time;            
//原点
Eigen::Vector3f origin;
//点云数据
TimedPointCloud ranges;
    //坐标
    Eigen::Vector3f position;
    //帧内时间
    float time;
//反射强度
std::vector<float> intensities;

---

2：LaserScan传感数据转换cartographer数据

cartographer::sensor::TimedPointCloudData ScanToTimedPointCloudData()
{

    cartographer::sensor::TimedPointCloudData time_point_cloud;
    time_point_cloud.time = cartographer::common::NowTime();
    cartographer::sensor::PointCloudWithIntensities point_cloud;

    float angle_min = -M_PI;
    float angle_max = M_PI;
    float angle = angle_min;
    float angle_increment = (angle_max - angle_min) / float(scanszie);
    float range_min = std::atof(driver_->getParametersCached().at("radial_range_min").c_str());
    float range_max = std::atof(driver_->getParametersCached().at("radial_range_max").c_str());
    float range_length = range_min;

    cartographer::sensor::TimedRangefinderPoint point;
    for (size_t i = 0; i < scanszie; ++i)
    {
        range_length = float(scandata.distance_data[i]) / 1000.0f;
        if (range_min <= range_length && range_length <= range_max)
        {
            point.position = Eigen::Vector3f(range_length * cos(angle), range_length * sin(angle), 0.);
            point_cloud.intensities.push_back(scandata.amplitude_data[i]);
        }
        else
        {
            point.position = Eigen::Vector3f::Zero();
            point_cloud.intensities.push_back(0.f);
        }
        point.time = 0;
        point_cloud.points.push_back(point);
        angle += angle_increment;
    }

    time_point_cloud.origin = Eigen::Vector3f::Zero();
    time_point_cloud.ranges = point_cloud.points;
    time_point_cloud.intensities = point_cloud.intensities;

    return time_point_cloud;
}

---

3：LaserScan传感数据转换Ros数据

【cartographer_ros】三： 发布和订阅雷达LaserScan信息

---

4：cartographer数据转换Ros数据

sensor_msgs::LaserScan ToRosMsg(cartographer::sensor::TimedPointCloudData time_point_cloud)
{
    sensor_msgs::LaserScan scan;

    scan.header.stamp = common::RosFromCarto(time_point_cloud.time);
    scan.header.frame_id = "base_link";
    scan.angle_min = -M_PI + ((slam::common::ConfigData.slam_scan_offset * M_PI) / 180.0);
    scan.angle_max = M_PI + ((slam::common::ConfigData.slam_scan_offset * M_PI) / 180.0);
    scan.angle_increment = (scan.angle_max - scan.angle_min) / float(common::ConfigData.scanner_per_scan);
    scan.scan_time = float(1.0 / float(common::ConfigData.scanner_frequency));
    scan.time_increment = float(1.0 / float(common::ConfigData.scanner_frequency * common::ConfigData.scanner_per_scan));
    scan.range_min = 0.0;
    scan.range_max = 100.0;
    scan.ranges.resize(common::ConfigData.scanner_per_scan);
    scan.intensities.resize(common::ConfigData.scanner_per_scan);
    for (int i = 0; i < common::ConfigData.scanner_per_scan; ++i)
    {
        scan.ranges[i] = sqrt(pow(time_point_cloud.ranges[i].position[0], 2) + pow(time_point_cloud.ranges[i].position[1], 2));
        scan.intensities[i] = time_point_cloud.intensities[i];
    }
    return scan;
}

---

5：Ros数据转换cartographer数据

cartographer::sensor::TimedPointCloudData ToCartoSensor(sensor_msgs::LaserScan scan)
{
    cartographer::sensor::TimedPointCloudData time_point_cloud;
    cartographer::sensor::PointCloudWithIntensities point_cloud;
    size_t scanszie = scan.ranges.size();
    float angle = scan.angle_min;
    float angle_increment = scan.angle_increment;
    float range_length = scan.range_min;

    cartographer::sensor::TimedRangefinderPoint point;
    for (size_t i = 0; i < scanszie; ++i)
    {
        range_length = scan.ranges[i];
        point.position = Eigen::Vector3f(range_length * cos(angle), range_length * sin(angle), 0.);
        point.time = 0;
        point_cloud.intensities.push_back(scan.intensities[i]);
        point_cloud.points.push_back(point);
        angle += angle_increment;
    }
    time_point_cloud.origin = Eigen::Vector3f::Zero();
    time_point_cloud.ranges = point_cloud.points;
    time_point_cloud.intensities = point_cloud.intensities;
    time_point_cloud.time = TimeCartoFromRos(scan.header.stamp);
    return time_point_cloud;
}

【完】

---

下一节会介绍里程计Odom数据转换为cartographer中的cartographer::sensor::OdometryData数据。