点云在ROS中的应用

点云在ROS中的应用

用RoboSense雷达节点，加载pcap发布原始数据

mkdir -p pointcloud_ws/src
cd src
git clone https://github.com/RoboSense-LiDAR/ros_rslidar
cd ..
catkin_make

下载并安装libpcap-dev

sudo apt install libpcap-dev

修改slidar_pointcloud\launch\rs_lidar_32.launch文件中的pacp文件目录

<param name="pcap" value="/media/ghigher/KINGIDISK/Datasets/CH1激光雷达/RS_32_data.pcap"/>

运行launch文件

roslaunch rslidar_pointcloud rs_lidar_32.launch 

rosbag录制激光雷达数据

运行激光雷达数据

roslaunch rslidar_pointcloud rs_lidar_32.launch 

查看话题rostopic list

/clicked_point
/cloud_node/parameter_descriptions
/cloud_node/parameter_updates
/diagnostics
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/rslidar_node/parameter_descriptions
/rslidar_node/parameter_updates
/rslidar_packets
/rslidar_packets_difop
/rslidar_points
/temperature
/tf
/tf_static

其中话题/rslidar_points就是需要录制的激光雷达数据

 rosbag record -o lidar_point.bag /rslidar_points

或者通过可视化界面录制

rqt_bag

bag查看

rosbag play lidar_point_2024-02-03-22-19-29.bag 

或者通过可视化界面查看

rqt_bag lidar_point_2024-02-03-22-19-29.bag

同步订阅雷达和图像话题

要求:

• 同步订阅激光雷达话题和图像话题
• 更改图像信息的header时间戳，与激光消息一致，并发布
• 录制发布的同步激光雷达和图像数据

ros官网提供了message_filters用于对齐多种传感信息的时间戳。

官方概述：http://wiki.ros.org/message_filters

message_filtersis a utility library for use with roscpp and rospy . It collects commonly used message “filtering” algorithms into a common space. A message filter is defined as something which a message arrives into and may or may not be spit back out of at a later point in time.

An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp.

#include "ros/ros.h"
#include <rosbag/bag.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <iostream>

std::string img_topic = "/ccf_pc/_img";
std::string point_topic = "/ccf_pc/_points";

rosbag::Bag bag_record; // bag文件记录

ros::Publisher img_pub;
ros::Publisher pointcloud_pub;

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> testSyncPoicy;
// 回调函数
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::PointCloud2ConstPtr &point)
{
    sensor_msgs::Image img_temp;
    img_temp = *image;
    img_temp.header.stamp = point->header.stamp; // 复制时间戳
    // 直接写入bag文件
    bag_record.write(img_topic, image->header.stamp.now(), image);
    bag_record.write(point_topic, point->header.stamp.now(), point);

    std::cout << "encoding..." << image->encoding << std::endl;
    // ROS_INFO("encoding: %s", image->encoding);
    ROS_INFO("step: %d", image->step);
    ROS_INFO("height: %d", image->height);
    ROS_INFO("width: %d", image->width);
    //实时发布
    img_pub.publish(image);
    pointcloud_pub.publish(point);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "time_sync");
    ros::NodeHandle n;

    bag_record.open("/home/ghigher/workplace/pointcloud_ws/src/time_sync_demo/bag/record.bag", rosbag::bagmode::Write); //写入路径
    //发布的话题
    img_pub = n.advertise<sensor_msgs::Image>(img_topic, 1000);
    pointcloud_pub = n.advertise<sensor_msgs::PointCloud2>(point_topic, 1000);

    message_filters::Subscriber<sensor_msgs::Image> img_sub(n, "/zed/zed_node/left/image_rect_color", 1); //topic1输入
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub(n, "/rslidar_points", 1);  //topic2输入

    message_filters::Synchronizer<testSyncPoicy> sync(testSyncPoicy(10), img_sub, pointcloud_sub); // 同步
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();
    ros::shutdown();
}

roscore
rosrun time_sync_demo time_sync 

运行bag文件

rosbag play seu.bag 

查看bag

rqt_bag record.bag

可以看出图像与激光雷达时间戳大致对齐，小部分差异是由程序bag写入时的先后顺序导致的。