ROS C++ IMU、视觉传感器 消息同时订阅及图片的保存

 文件放在了如下目录：

 

一、buaa_imu

cd my_c++/VINS_test/BUAA_robot/src/Sensors/src

catkin_create_pkg buaa_imu roscpp sensor_msgs  tf

buss_imu 是我们建立的包，主要的功能是用于IMU信息的读取和主题的发布；IMU传感器与电脑通过串口USB连接；

<launch>
  <node name="imu_read_node" pkg="buaa_imu"  type="imu_read" output="screen">
    <param name="com_port"     value="/dev/imu"/>
    <param name="imu_frame_id" value="imu_link"/>
  </node>
</launch>

imu.launch 里面的串口名称要根据电脑识别的 去改一下

 

二、pylon_camera

pylon_camera 网上下载的相机包，用于相机信息的采集和主题的发布

Basler相机是德国生产的工业相机，有USB和 Gie两种连接模式，本人用的是网线连接；

三、buaa_bringup

建立的包，完成IMU、相机信息的发布和保存；
相当于其他launch 文件的调用

cd my_c++/VINS_test/BUAA_robot/src/Sensors/src

catkin_create_pkg buaa_bringup roscpp  geometry_msgs  tf  nav_msgs
-------------------------------------------------------------------------------------------------

buaa_bringup 包中的信息

1、test.launch

<launch>
 
  <!-- buaa bring up -->
  <include file="$(find buaa_bringup)/launch/minimal.launch"/>

  <!-- launch imu -->
  <include file="$(find buaa_imu)/launch/imu.launch" />

  <!-- launch balser_cam -->
  <include file="$(find pylon_camera)/launch/pylon_camera_node.launch" />

</launch>

roslaunch buaa_bringup test.launch

一条语句可以启动三个包，学习一下吧

2、CmakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(buaa_bringup)

 
find_package(catkin REQUIRED COMPONENTS
  OpenCV
  cv_bridge
  image_transport
  roscpp
  rospy
  std_msgs
)

  
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES minicar_ctrl
#  CATKIN_DEPENDS roscpp
#  DEPENDS system_lib
)

 
include_directories(
  include
 
  ${catkin_INCLUDE_DIRS}
  
  ${OpenCV_INCLUDE_DIRS}
)

find_package(OpenCV)
include_directories(include ${OpenCV_INCLUDE_DIRS})

find_package( OpenCV 3.4.1 REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller ${catkin_LIBRARIES} ${OpenCV_LIBS})
 

3、package.xml

<?xml version="1.0"?>
<package format="2">
  <name>buaa_bringup</name>
  <version>0.0.0</version>
  <description>The buaa_bringup package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="hltt3838@todo.todo">hltt3838</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/buaa_bringup</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>OpenCV</build_depend>

  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>OpenCV</build_export_depend>
  
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>image_transport</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>OpenCV</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

 

4、.cpp

#include <ros/ros.h>
#include <queue>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
 
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
 
#include <sensor_msgs/Imu.h>
#include <opencv2/opencv.hpp>
 
 
using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace geometry_msgs;
 
queue<sensor_msgs::Imu> imu_buf;
queue<sensor_msgs::Image> feature_buf;

string img_topic = "/pylon_camera_node/image_raw";
string imu_topic = "imu";

string  cam0_path  = "/home/hltt3838/my_c++/VINS_test/BUAA_robot/cam0/" ;   
string  cam1_path  = "/home/hltt3838/my_c++/VINS_test/BUAA_robot/cam1/" ;

double  image_time;
string image_name;

 void imu_callback(const sensor_msgs::Imu &imu_msg)
{
 
    imu_buf.push(imu_msg);
   
}

void feature_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
   image_time = img_msg->header.stamp.toSec();
   ROS_INFO("send msg = %d", image_time);
   //读取sensor_msgs::Image img的数据，并转为MONO8格式，用cv::Mat show_img接收。
    cv_bridge::CvImageConstPtr ptr;
    //ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
    ptr = cv_bridge::toCvCopy(img_msg, "bgr8");
    cv::Mat show_img = ptr->image;
    image_name = std::to_string(image_time) + ".jpg";               //图像命名：时间戳.jpg
    cv::imwrite(cam0_path + image_name, show_img);  //保存图片
    //feature_buf.push(img_msg);
    //图像显示程序
    cv::namedWindow("vis");
    cv::imshow("vis", show_img);
    cv::waitKey(5);//注释后只有数据，没有图像产生
}



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

  //创建消息订阅者，订阅IMU、feature、restart、match_points的topic，执行各自回调函数
  //ros::Subscriber sub_imu = n.subscribe(imu_topic, 2000, imu_callback);
                                        
  //这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息
  ros::Subscriber sub_image = n.subscribe(img_topic, 2000, feature_callback);   
  
  ros::spin();
  
  return 0;
  

目前存在的问题是不能把读取的图像放在指定的文件夹中，帮忙看一下问题，本人正在查找！

已经解决，可以实现捕获图像的显示，以及图片的保存,而且是以时间为名字保存！

 

四、程序运行

1、打开第一个终端
roscore

2、打开第二个终端

cd my_c++/VINS_test/BUAA_robot/src/Sensors
//catkin_make  // 这一步没有编译成功，用的是

catkin_make -DCATKIN_WHITELIST_PACKAGES=""
source  devel/setup.bash
//roslaunch buaa_bringup all_sensor.launch
roslaunch buaa_bringup test.launch

3、打开第三个终端
rosrun image_view image_view image:=/pylon_camera_node/image_raw
或者
rviz

看看相关文件有没有保存图片信息