利用ROS同时采集激光雷达、摄像头数据并提取信息

ros 同时被 2 个专栏收录
9 篇文章 0 订阅
7 篇文章 0 订阅

利用ROS同时采集激光雷达、摄像头数据并提取信息

在实际工程中,往往需要采集lidar和camera的信息,并进行同步,而二者的频率往往不一致,比如相机的采集频率为30HZ,而velodyne vlp 16的采集频率为10HZ,通过ros采集信息之后还需要提取其中的图片和点云,转换为视频和PCD等格式,以便于后续的使用。

pcl_ros官方教程: pcl_ros官方教程

将bag包转为视频代码: bag2video

1.采集点云和图像信息

1.1 利用opencv采集图像

采集图像信息是通过opencv读取摄像头信息,并转成sensor_msgs::Image格式发布出来的,具体代码如下:

  • camera_node.cpp
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <stdio.h>
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "camera");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("velodyne/image", 1);
 
  cv::VideoCapture cap(0);
  std::string img_name;
  if(!cap.isOpened())
  {
    ROS_INFO("Can not open camera!");
    return -1;
  }
  else
  {
    ROS_INFO("Camera opened success...");
  }
  cap.set(CV_CAP_PROP_FRAME_WIDTH ,640);
  cap.set(CV_CAP_PROP_FRAME_HEIGHT,480);
  cv::Mat frame;
  ros::Rate loop_rate(30);
  
  while(nh.ok()&&cap.isOpened())
  {
    {
      cap>>frame;
      if(frame.empty())
      {
        ROS_INFO("frame is empty!");
        return -1;
      }
      else
      {
      cv::imshow("camera frame",frame);
      if(cv::waitKey(10)==27)
      {
        ROS_INFO("ImagePro ESC...");
        return -1;
      }
      //发布图像消息
      sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();  //通过cv_bridge转换成sensor_msgs::ImagePtr格式
      pub.publish(msg);
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
  • CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(opencv_test)
find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  image_transport
  roscpp
  rospy
  sensor_msgs
  std_msgs
)

find_package(OpenCV REQUIRED)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
add_executable(camera_node src/test_cv.cpp)
target_link_libraries(camera_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

1.2 采集过程

本文所采用的是velodyne vlp 16的激光雷达和索尼的摄像头,故通过相关的代码驱动,将点云和图像信息通过topic发布出来,具体如下所示。

roslaunch velodyne_pointcloud VLP16_points.launch  
rosrun opencv_test camera_node
rosbag record -b 4096 -o 1st.bag /velodyne_points /velodyne/image

注:

  • 要先安装velodyne的驱动包,具体可以参考:Velodyne VLP-32C/VLP-16在ROS下的数据获取、显示及录制
  • 其中rosbag record 的参数 -b 4096是设置保存的缓冲区大小,但在具体实验过程中,还是由于需要保存的信息太大,而造成rosbag包没有录制完成,所以很多时候就拆成了许多小的bag包来保存。

采集过程可通过rviz可视化显示:

在这里插入图片描述

2. 从bag包中提取信息

经过上文采集的bag包中的图片和点云信息均为sensor_msgs格式,怎么才能再将其中的图片和点云信息提取出来呢?可以通过直接读bag包或者接收对应的话题来实现。

2.1 提取时间戳

可以直接通过读取bag包,获取每一帧数据的时间戳,放在一个列表里,再将其保存即可.

  • timestamp_extractor.py
#!/usr/bin/python
# coding:utf-8
import roslib
import rosbag
import rospy
import pandas as pd
import csv
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

if __name__ == '__main__':
    timestamps=[]
    save_root_path = '/home/young/catkin_calib/src/extract_bag_info/data/'
    bag_path = '/media/young/YOUNG/1st.bag'
    
    try:
        with rosbag.Bag(bag_path,'r') as bag:
            for topic,msg,t in bag.read_messages():
                if topic == "/velodyne_points":
                    timestamp = msg.header.stamp.to_sec()
                    timestamps.append(timestamp)
                    print 'timestamp:',timestamp
    except rospy.ROSInterruptException:
        pass
        if len(timestamps):
            print '1111111111111111111111111111111'
            timestamps_df = pd.DataFrame(timestamps)
            timestamps_df.to_csv(save_path,sep=' ',index=False,header=False,quoting=csv.QUOTE_NONE,escapechar=' ')
            print 'save success!'
            timestamps *= 0  # 清空列表  不能用 timestamps.clear()会报错

2.2 将点云文件转为PCD

利用ros自带功能包pcl_ros,具体可参见: http://wiki.ros.org/pcl_ros#ROS_nodes

roscore
rosrun pcl_ros bag_to_pcd /media/young/YOUNG/1st.bag /velodyne/image PCD

2.3 提取图片

2.3.1 利用ros工具提取图片

采用ros官方给的功能包imgae_view。具体可以参见:http://wiki.ros.org/image_view

  1. image_view是什么?

    A simple viewer for ROS image topics. Includes a specialized viewer for stereo + disparity images.意思就是,image_view是用于查看立体+视差图的一个图像查看器。

  2. image_view包含的工具

    1. image_saver

      用于从sensor_msgs/Image流中将图片保存为jpg/png格式。

      参数:

      	1. _filename_format:  默认为  <font color='#004682b4'>left%04d.%s</font>
      	2. _encoding:默认为 <font color='#004682b4'>'bgr8'</font>
      	3. _save_all_image:默认 <font color='#004682b4'>True</font>
      

      用法:

      rosrun image_view image_saver image:=/velodyne/image
      
    2. extract_images

      和 image_saver类似,将图像以jpg/png格式从(ROS sensor_msgs/Image话题)中保存到文件中。

      参数:

      1. _filename_format:  默认为  <font color='#004682b4'>left%04d.jpg</font>
       	2. _sec_per_frame:设置每一帧之间的时间差,默认为 <font color='#004682b4'>0.1</font>
      

      用法:

      rosrun image_view extract_images _sec_per_frame=0.01 image:=/velodyne/image
      
    3. video_recorder

      可以将(ROS sensor_msgs/Image 话题)记录到文件中,依赖于opencv的VideoWriter类,默认选项将编码为MPG格式,以15fps保存为.avi格式,最后生成一个output.avi的文件。

      参数:

      1. _filename:  默认为  <font color='#004682b4'>outpu.avi</font>
      2. _fps:默认为  <font color='#004682b4'>15</font>
      3. _codec: 图像编码器格式,默认为<font color='#004682b4'>MJPG</font>
      4. _encoding: 视频图像色彩空间 ,默认为 <font color='#004682b4'>bgr8</font>
      

      用法:

      rosrun image_view video_recorder _fps:=30 image:=/velodyne/image _filename:=output.avi
      

当采用extract_images提取图片时,可得到一系列 frame*.jpg的图像,具体如下所示:
在这里插入图片描述

2.3.2 通过cv_bridge转成图像保存

通过cv_bridge将sensor_msgs::Image格式的图像转换opencv格式的图像,并显示出来和保存,具体代码如下,输入保存图片的路径,播放bag即可。

  • extract_img.py
#!/usr/bin/python
# coding:utf-8

import sys
import roslib
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

count=0

def img_cb(msg):
    global root_path,count
    count = count + 1
    bridge = CvBridge()
    img_path = root_path + str(count).zfill(4)+'.png'
    
    cv_image = bridge.imgmsg_to_cv2(msg,"bgr8")#图像转换
    cv2.imshow('imshow',cv_image)
    cv2.imwrite(img_path, cv_image)
    cv2.waitKey(10)
    #cv2.destroyAllWindows()

def main(argv):
    global root_path
    rospy.init_node('extract_img_node',anonymous=True)
    root_path = argv[1] + '/'
    print 'hahaha'
    rospy.Subscriber("/velodyne/image", Image, img_cb)
    rospy.spin()


if __name__ == '__main__':
    main(sys.argv)
  • 运行过程
roscore
python extract_img.py /media/young/YOUNG/test1
rosbag play 1st.bag

保存结果如下:

在这里插入图片描述

2.4 提取视频

2.4.1 利用ros工具提取视频

如上文所述,可以通过ros自带功能包image_view中的video来提取视频,具体如下。

roscore
rosrun image_view video_recorder _fps:=30 image:=/velodyne/image _filename:=output.avi
rosbag play 1st.bag

提取出的视频如下:

在这里插入图片描述

但通过rosbag info指令得到的bag包信息为:

path:        1st.bag
version:     2.0
duration:    1:06s (66s)
start:       Nov 18 2020 08:17:15.97 (1605658635.97)
end:         Nov 18 2020 08:18:22.61 (1605658702.61)
size:        2.1 GB
messages:    2650
compression: none [1989/1989 chunks]
types:       sensor_msgs/Image       [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:      /velodyne/image    1989 msgs    : sensor_msgs/Image      
             /velodyne_points    661 msgs    : sensor_msgs/PointCloud2

可见,最后导出的视频时长为45s,而bag包中的时长为1分06秒,不能完整的还原视频的时长信息,和点云的时间不匹配。按照网上的其他方法,比如利用ffmpeg将图片转为视频,也不能得到很好的效果。

2.4.2 利用ffmpeg工具提取

sudo apt-get install ffmpeg  # 安装 ffmpeg
# 在图片文件夹下
ffmpeg -r 30 -i frame%04d.jpg ../test.avi
# 也可以使用如下指令
# ffmpeg -framerate 30 -i frame%04d.jpg -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p ../test.avi

得到的视频如下所示,也没有和bag包中的时长相同。

在这里插入图片描述

2.4.3 利用ffmpeg和opencv提取视频

看到一个大牛写的,具体可以参考:bag2video 感谢大牛!

roscore
cd catkin_ws/src
git clone https://github.com/mlaiacker/rosbag2video.git
# 根据你的bag包在哪儿,可以将 ~/catkin_ws/src/rosbag2video/rosbag2video.py拷贝到bag包目录下
sudo cp ~/catkin_ws/src/rosbag2video/rosbag2video.py /media/young/YOUNG/1st.bag
cd /media/young/YOUNG
# 如果python2.7可以使用opencv,则执行
python rosbag2video.py 1st.bag
# 否则按照python2的opencv,或python3的opencv,官方以python3运行,则
sudo apt install python3-roslib python3-sensor-msgs python3-opencv
python3 rosbagvideo.py 1st.bag
# 最后会生成一个 <bagname>_<your_topic_name>.bag的一个avi文件

最后提取的视频结果如下:

在这里插入图片描述

  • 0
    点赞
  • 6
    评论
  • 8
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

相关推荐
©️2020 CSDN 皮肤主题: 深蓝海洋 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值