本周学习汇报

风吹杨柳岸

已于 2022-06-26 21:18:33 修改

阅读量485

点赞数 2

文章标签：学习自动驾驶人工智能

于 2022-06-26 21:07:09 首次发布

本文链接：https://blog.csdn.net/qq_49589440/article/details/125473382

版权

1、编写函数，获取雷达数据

#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>


//声明一个类
class LaserScan
{
private:
    ros::NodeHandle node_handle_;
    ros::NodeHandle private_node_;
    ros::Subscriber laser_scan_subscriber_;
public:
    LaserScan();
    ~LaserScan();
    void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
//构造函数
LaserScan::LaserScan():private_node_("~")
{

    laser_scan_subscriber_ = node_handle_.subscribe("laser_scan",1,& LaserScan::ScanCallback,this);
}

LaserScan::~LaserScan()
{
}

//回调函数
void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // 通过ranges中数据的个数进行雷达数据的遍历
    for (int i = 0; i < scan_msg->ranges.size(); i++)
     {
    double range = scan_msg->ranges[i];
    double angle = scan_msg->angle_min + scan_msg->angle_increment * i;
    double x = range * cos(angle);
    double y = range * sin(angle);

    ROS_INFO_STREAM(
        // 第i个数据点对应的极坐标为: 
        "range = " << range << ", angle = " << angle << 
        // 第i个数据点对应的欧式坐标为: 
        ", x = " << x << ", y = " << y
    );
     }

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson1_laser_scan_node"); // 节点的名字
    LaserScan laser_scan;

    ros::spin();    // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
    return 0;
}

将得到的雷达数据的水平旋转角度和距离，转换为笛卡尔坐标，呈现出二维点云的效果，并且方便后面的计算，效果如下：

点云数据

2、编写ICP算法计算相邻2帧雷达数据间坐标变换，得到机器人位姿数据

icp算法原理：
1、对原始点云进行采样；采样方法有：均匀采样、随机采样、法矢采样
2、确定初始对应点集；方法有：点到点，点到线，点到面
3、去除错误对应点对；是icp算法的难点，也是改进icp算法的关键，没做
4、求解坐标变换,得到相邻两帧数据的（R,t)；方法：svd奇异值法

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <chrono>
class ScanMatchICP
{
    // 使用PCL中点的数据结构 pcl::PointXYZ
    typedef pcl::PointXYZ PointT;
    // 使用PCL中点云的数据结构 pcl::PointCloud<pcl::PointXYZ>
    typedef pcl::PointCloud<PointT> PointCloudT;
private:
    ros::NodeHandle node_handle_;           // ros中的句柄
    ros::NodeHandle private_node_;          // ros中的私有句柄
    ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber

    bool is_first_scan_;    // 判断是否是第一个雷达数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr current_pointcloud_;    // 当前帧雷达数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr last_pointcloud_;       // 前一帧雷达数据

    // icp算法
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;

    void ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
    void ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg);

public:
    ScanMatchICP();
    ~ScanMatchICP();
    void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
//构造函数
ScanMatchICP::ScanMatchICP()
{
    // \033[1;32m，\033[0m 终端显示成绿色
    ROS_INFO_STREAM("\033[1;32m----> Scan Match with ICP started.\033[0m");
    laser_scan_subscriber_ = node_handle_.subscribe(
        "laser_scan", 1, &ScanMatchICP::ScanCallback, this);
    // 第一帧数据的标志
    is_first_scan_ = true;
    // 指针的初始化
    current_pointcloud_ = boost::shared_ptr<PointCloudT>(new PointCloudT());//当前帧雷达数据转换成pcl点云格式后的数据
    last_pointcloud_ = boost::shared_ptr<PointCloudT>(new PointCloudT());//上一帧雷达数据转换成pcl点云格式后的数据
}
//系构函数
ScanMatchICP::~ScanMatchICP()
{
    ROS_INFO("Destroying ScanMatchICP");
}
//回调函数
void ScanMatchICP::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // step1 进行数据类型转换
    std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
    // 对第一帧数据进行特殊处理
    if (is_first_scan_ == true)
    {
        // 进行第一帧数据的处理,只转换数据类型 并 保存到current_pointcloud_
        ConvertScan2PointCloud(scan_msg);
        is_first_scan_ = false;
        return;
    }
    else    
        // 在将新一帧数据转换到当前帧之前,
        // 先将current_pointcloud_赋值到last_pointcloud_进行保存
        *last_pointcloud_ = *current_pointcloud_;   

    // 进行数据类型转换
    ConvertScan2PointCloud(scan_msg);

    std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
    std::cout << "\n转换数据格式用时: " << time_used.count() << " 秒。" << std::endl;

    // step2 使用ICP计算 雷达前后两帧间的坐标变换
    start_time = std::chrono::steady_clock::now();

    // 调用ICP进行计算
    ScanMatchWithICP(scan_msg);

    end_time = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
    std::cout << "ICP计算用时: " << time_used.count() << " 秒。" << std::endl;
}

// 将LaserScan消息类型转换为PCL的pcl::PointCloud类型
void ScanMatchICP::ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // PointCloudT::Ptr的数据类型为boost::shared_ptr
    PointCloudT::Ptr cloud_msg = boost::shared_ptr<PointCloudT>(new PointCloudT());
    // 对容器进行初始化
    cloud_msg->points.resize(scan_msg->ranges.size());

    for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
    {
        // 首先声明一个 cloud_msg第i个点的 引用
        pcl::PointXYZ &point_tmp = cloud_msg->points[i];
        // 获取scan的第i个点的距离值
        float range = scan_msg->ranges[i];

        // 将 inf 与 nan 点 设置为无效点
        if (!std::isfinite(range))
            continue;

        // 有些雷达驱动会将无效点设置成 range_max+1
        // 所以要根据雷达的range_min与range_max进行有效值的判断
        if (range > scan_msg->range_min && range < scan_msg->range_max)
        {
            // 获取第i个点对应的角度
            float angle = scan_msg->angle_min + i * scan_msg->angle_increment;
            // 获取第i个点在笛卡尔坐标系下的坐标
            point_tmp.x = range * cos(angle);
            point_tmp.y = range * sin(angle);
            point_tmp.z = 0.0;
        }
    }

    // 高度为1的情况下, width即为所有点的个数
    cloud_msg->width = scan_msg->ranges.size();
    cloud_msg->height = 1;
    cloud_msg->is_dense = true; // not contains nans

    // 将scan_msg的消息头 赋值到 pcl::PointCloud<pcl::PointXYZ>的消息头
    pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);

    // 将转换完的数据赋值到current_pointcloud_中保存下来
    *current_pointcloud_ = *cloud_msg;
}

/*
 * 调用ICP进行帧间位姿的计算
 */
void ScanMatchICP::ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // ICP 输入数据,输出数据的设置,还可以进行参数配置,这里使用默认参宿，算法关键
    icp_.setInputSource(last_pointcloud_);
    icp_.setInputTarget(current_pointcloud_);
    // 开始迭代计算
    pcl::PointCloud<pcl::PointXYZ> unused_result;
    icp_.align(unused_result);

    // std::cout << "has converged:" << icp_.hasConverged() << " score: " << icp_.getFitnessScore() << std::endl;

    // 如果迭代没有收敛,不进行输出
    if (icp_.hasConverged() == false)
    {
        std::cout << "not Converged" << std::endl;
        return;
    }
    else
    {
        // 收敛了之后, 获取坐标变换
        Eigen::Affine3f transfrom;
        transfrom = icp_.getFinalTransformation();

        // 将Eigen::Affine3f转换成x, y, theta, 并打印出来
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);
        std::cout << "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl;
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson2_scan_match_icp_node"); // 节点的名字
    ScanMatchICP scan_match_icp;

    ros::spin(); // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
    return 0;
}

3、发布栅格地图，构建栅格地图

栅格地图的消息类型：
header:存储数据的时间戳，地图的坐标系
info:加载地图的时间，地图的分辨率，地图的宽度与高度
data：地图数据


#include <iostream>
#include <chrono>

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

class OccupancyGrid
{
private:
    ros::NodeHandle node_handle_;           // ros中的句柄
    ros::Publisher map_publisher_;          // 声明一个Publisher
    ros::Publisher map_publisher_metadata_; // 声明一个Publisher
    nav_msgs::OccupancyGrid map_;           //用来发布map的实体对象

    std::chrono::steady_clock::time_point start_time_;
    std::chrono::steady_clock::time_point end_time_;
    std::chrono::duration<double> time_used_;

public:
    OccupancyGrid();
    void PublishMap();
};

// 构造函数
OccupancyGrid::OccupancyGrid()
{
    // \033[1;32m，\033[0m 终端显示成绿色
    ROS_INFO_STREAM("\033[1;32m----> Make Occupancy Grid Map by no move started.\033[0m");

    map_publisher_ = node_handle_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    map_publisher_metadata_ = node_handle_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);

    // 对map_进行初始化
    map_.header.frame_id = "map";

    // 地图的分辨率为0.05m,代表一个格子的距离是0.05m
    map_.info.resolution = 0.05;

    // 地图图片像素的大小, width为地图的宽度是多少个像素
    map_.info.width = 20;
    map_.info.height = 20;

    // 如果要表示地图图片为多少米的话,就需要用实际长度除以分辨率,得到像素值
    // map_.info.width = 100 / map_.info.resolution;
    // map_.info.height = 100 / map_.info.resolution;

    // 地图左下角的点对应的物理坐标
    map_.info.origin.position.x = 0.0;
    map_.info.origin.position.y = 0.0;

    // 对数组进行初始化, 数组的大小为实际像素的个数
    map_.data.resize(map_.info.width * map_.info.height);
}

// 构造map并进行发布
void OccupancyGrid::PublishMap()
{
    start_time_ = std::chrono::steady_clock::now();

    // 通过二维索引算出来的一维索引
    int index = 0;

    // 10种情况
    int count = 10;

    // 固定列, 优先对行进行遍历
    for (int j = 0; j < map_.info.height; j++)
    {
        for (int i = 0; i < map_.info.width; i++)
        {
            // 二维坐标转成一维坐标
            index = i + j * map_.info.width;
            // std::cout << " index: " << index ;

            // 0代表空闲, 100代表占用, -1代表未知, 默认值为0

            // 为map赋予不同的值来体验效果, 从-1 到 254
            if (index % count == 0)
                map_.data[index] = 100;
            else if (index % count == 1)
                map_.data[index] = 100;
            else if (index % count == 2)
                map_.data[index] = 100;
            else if (index % count == 3)
                map_.data[index] = 100;
            else if (index % count == 4)
                map_.data[index] = 100;
            else if (index % count == 5)
                map_.data[index] = 100;
            else if (index % count == 6)
                map_.data[index] = 100;
            else if (index % count == 7)
                map_.data[index] = 100;
            else if (index % count == 8)
                map_.data[index] = 100;
            else if (index % count == 9)
                map_.data[index] = 100;
        }
    }
    
    // 设置这一帧地图数据的时间戳
    map_.header.stamp = ros::Time::now();

    // 发布map和map_metadata话题
    map_publisher_.publish(map_);
    map_publisher_metadata_.publish(map_.info);

    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "发布一次地图用时: " << time_used_.count() << " 秒。\n" << std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson4_make_occupancy_grid_map");
    OccupancyGrid occupancy_grid;
    ros::Rate rate(1);

    while (ros::ok())
    {
        ROS_INFO("publish occupancy map");
        occupancy_grid.PublishMap();
        rate.sleep();
    }

    return (0);
}