ROS2 bag中自定义msg信息转为rviz中显示信息

实际开发中，往往会参考传感器信息类型需要，定义专门的msg信息，然后提取其中的有用数据用于后续的计算开发。

试验过程中采集的各种场景下的ROSbag也非常多，经常会在某个场景采集多次，有时候会希望在PC端实现采集场景的信息复现，ROS中常用的可视化显示中包括rviz。

本文简述一个小功能，在ROS2的rviz中显示出自定义msg信息。

原理主要是，将自定义bag里面msg中自定义的信息解析并提取出来（subscription），赋值给PCL格式下的例如PointCloud<pcl::PointXYZ>，然后再将PCL格式的点云以rviz中常用的数据格式显示出来。

首先iclude所有涉及到的msg编译好的.h文件

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "yourmsg/msg/pose_estimation.hpp"
#include <pcl/point_types.h>

 再者，PCL点云格式转化成rviz中可显示的std::msg信息，需要用到转换工具：

#include "pcl_conversions/pcl_conversions.h"

如果没有安装PCL工具的需要提前先安装对应ROS版本的PCL工具。

sudo apt install ros-你的ROS版本代号-pcl*

之后就是按照原理所述，先订阅bag中的msg信息，解析为基本数据结构（例如结构体），提取其中的有用信息赋值给PCL中的点云格式点，在将其转化为std::msg

class Ros2bagshow: public rclcpp::Node
{
public:
    Ros2bagshow(): Node("ros2bag_show")
    {
        subscription_SLAM = this->create_subscription<yourmsg::msg::PoseEstimation>("fuse_localization_pub", 10, std::bind(&Ros2bagshow::topic_callback, this, std::placeholders::_1));
        publisher_SLAM = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_cloud_topic", 10);
    }
    
private:
    rclcpp::Subscription<yourmsg::msg::PoseEstimation>::SharedPtr subscription_SLAM;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_SLAM;

    void topic_callback(const yourmsg::msg::oseEstimation::SharedPtr msg){
        sensor_msgs::msg::PointCloud2 std_msg;
        pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
        pcl::PointXYZ msg_p;

        msg_p.x = msg->x;
        msg_p.y = msg->y;
        msg_p.z = msg->z;
        std::cout << msg_p.x << " " << msg_p.y << " " << msg_p.z << std::endl;

        pcl_cloud.push_back(msg_p); 
        pcl::toROSMsg(pcl_cloud, std_msg);
        std_msg.header.frame_id = "ros2bag_show_frame";
        publisher_SLAM->publish(std_msg);
    }
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Ros2bagshow>();
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

编译成功能包，之后再将其与rosbag一起运行之后就可以完成新格式的数据解析和发布，再打开rviz就可以在显示的topic中找到对应的项目选中显示即可。