针对2021.3以来gazebo中Lidar点云数据不能正常使用的问题解决办法

重要补充写在前面

发现了一篇更好的彻底解决问题的文章，大家可以直接去参考这一篇了：
Ubuntu18.04下Gazebo与Loam算法实时仿真报错的解决办法

问题的出现

gazebo一直是一个重要的机器人领域的仿真软件，之前我写过一篇使用gazebo和LOAM算法进行联合运行的文章:
在Ubuntu18.04中使用gazebo配合LOAM算法仿真
在那时候gazebo还是可以正常使用的，但是今年三月之后，突然周围很多人反应gazebo和任何一个LOAM算法联合运行都会报错。我试了一下，果然是这样！
经过控制变量分析，知道一定是gazebo出了问题（版本低了？）

后来使用之前写过的一些点云处理的代码，发现

pcl::fromROSMsg(*CloudMsg, *point_ptr); 

这句代码根本运行不了（相信很多算法都有这一句），后来查了很多原因，目前认为主要原因是：
gazebo中得到的点云消息不安全，无法进行指针的转换

问题的思考

所以想了一个比较笨的办法：把gazebo中得到的点云录制为一个rosbag包，编写程序，将sensor_msgs::PointCloud2转化为sensor_msgs::PointCloud消息格式（这个倒是可以正常转换），然后得到PointXYZ格式点云，自己再发布出来。
所以这个解决办法只能离线跑，使用rosbag跑，不能同时运行gazebo和算法

代码以及使用方法

源代码（因为直接在之前代码基础上改的，所以请忽略一些不正确命名和注释）：

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>

class pcl_colored
{
private:
  ros::NodeHandle n;
  ros::Subscriber subCloud;
  ros::Publisher pubCloud;
  sensor_msgs::PointCloud2 msg;  //接收到的点云消息
  sensor_msgs::PointCloud2 colored_msg;  //等待发送的点云消息

public:
  pcl_colored():
    n("~"){
    subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_point", 1000, &pcl_colored::getcloud, this); //接收velodyne点云数据，进入回调函数getcloud()
    pubCloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 1000);  //建立了一个发布器，主题是/adjustd_cloud，方便之后发布加入颜色之后的点云
  }

  //回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    // std::cout << "ok1" <<std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr  colored_pcl_ptr (new pcl::PointCloud<pcl::PointXYZ>);   //放在这里是因为，每次都需要重新初始化，舍弃了原有但没用的两个通道intensity、ring
    // pcl::PointCloud<pcl::PointXYZ>::Ptr   raw_pcl_ptr (new pcl::PointCloud<pcl::PointXYZ>);   //VLP-16的点云消息包含xyz和intensity、ring的，这里没有加ring不知道为啥也可以，需要的话要自己定义一个点类型PointXYZIR
    sensor_msgs::PointCloud raw_pcl_ptr;
    sensor_msgs::convertPointCloud2ToPointCloud(*laserCloudMsg, raw_pcl_ptr);

    for (int i = 0; i <  raw_pcl_ptr.points.size(); i++)
    {
      pcl::PointXYZ  p;
      p.x=raw_pcl_ptr.points[i].x;
      p.y=raw_pcl_ptr.points[i].y;
      p.z=raw_pcl_ptr.points[i].z;
      colored_pcl_ptr->points.push_back(p);
    }
    colored_pcl_ptr->width = 1;
    colored_pcl_ptr->height = raw_pcl_ptr.points.size();
    pcl::toROSMsg( *colored_pcl_ptr,  colored_msg);  //将点云转化为消息才能发布
    colored_msg.header.frame_id = "velodyne";//帧id改成和velodyne一样的
    colored_msg.header.stamp = laserCloudMsg->header.stamp;//转化之后时间戳不变
    pubCloud.publish( colored_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

  ~pcl_colored(){}

};

int main(int argc, char** argv) {

  ros::init(argc, argv, "colored");  //初始化了一个节点，名字为colored

  pcl_colored  pc;

  ros::spin();
  return 0;
}

使用方法：

正常编译运行
播放你录制好的gazebo中的点云包，注意原来点云主题为/velodyne_points，而我们希望新得到的点云也是这个主题，但这俩不能相同，所以程序里面设定原本点云主题改为/velodyne_point，所以在播放包的时候要这样写

rosbag play ***.bag /velodyne_points:=/velodyne_point

于此同时记录你的新的包

rosbag record -O ****.bag /velodyne_points

之后就可以使用这个新的bag跑任意的算法了。