使用ros发布UVC相机和串口IMU数据

１．目的：为了可以标定普通USB相机和固定在相机上的外置IMU的外参,我希望通过ROS获取更高分辨率和更高频率的图像数据,并且可以将图像和imu的topic发布出来,直接使用rosbag record录制话题数据,写入bag文件,这样获得的bag文件直接可以用于相机和IMU的外参标定, 标定工具是kalibr.

2. 为了达到上述目的,我首先是完成使用ros发布出来从串口获取的imu数据,目前获取的频率是200hz,也是从网上找到的一个ros中串口通信的小demo,     参考   https://blog.csdn.net/tansir94/article/details/81357612  和   https://blog.csdn.net/xinmei4275/article/details/85040164?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase

3. 我安装了minicom  串口小工具

sudo apt-get install minicom
sudo minicom -D /dev/ttyUSB0 -b 230400 -H -w
-D 设置串口
-b 设置波特率
-H 设置十六进制显示
-w 自动换行

我的代码如下

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <serial/serial.h> //ROS已经内置了的串口包
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
serial::Serial ser; //声明串口对象

sensor_msgs::Imu imu_data;
//回调函数
void write_callback(const std_msgs::String::ConstPtr &msg) {
  ROS_INFO_STREAM("Writing to serial port" << msg->data);
  ser.write(msg->data); //发送串口数据
}
int main(int argc, char **argv) {
  //初始化节点
  ros::init(argc, argv, "serial_example_node");
  //声明节点句柄
  ros::NodeHandle nh;
  //订阅主题，并配置回调函数
  ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback);
  //发布主题
  ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
  ros::Publisher IMU_read_pub =
      nh.advertise<sensor_msgs::Imu>("imu_data", 1000);

// ros::Publisher Image_read_pub =
//       nh.advertise<sensor_msgs::>("imu_data", 1000);
  try {
    //设置串口属性，并打开串口
    ser.setPort("/dev/ttyUSB0");
    ser.setBaudrate(230400);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  } catch (serial::IOException &e) {
    ROS_ERROR_STREAM("Unable to open port ");
    return -1;
  }
  //检测串口是否已经打开，并给出提示信息
  if (ser.isOpen()) {
    ROS_INFO_STREAM("Serial Port initialized");
  } else {
    return -1;
  }
  //指定循环的频率
  ros::Rate loop_rate(400);
  while (ros::ok()) {

    unsigned char data_size;
    /*! Return the number of characters in the buffer. */
    // available()
//     while((data_size = ser.available()) < 11){
          
//     } 
     if (data_size = ser.available())
     {
      unsigned char tmpdata[data_size];
      ser.read(tmpdata, data_size);
      // std::cout << "data_size: " << data_size << std::endl;
      
      for (int i = 0; i < data_size; i++) {
        if (tmpdata[i] == 0x55) {
          switch (tmpdata[i + 1]) {
          case 0x51:
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            imu_data.linear_acceleration.x =
                ((short)(tmpdata[3 + i] << 8 | tmpdata[2 + i])) / 32768.0 * 4 * 9.7803;
            imu_data.linear_acceleration.y =
                ((short)(tmpdata[5 + i] << 8 | tmpdata[4 + i])) / 32768.0 * 4 * 9.7803;
            imu_data.linear_acceleration.z =
                ((short)(tmpdata[7 + i] << 8 | tmpdata[6 + i])) / 32768.0 * 4 * 9.7803;
            std::cout<<"acc: "<<std::setprecision(16)<<imu_data.header.stamp<<" " <<imu_data.linear_acceleration.x<<" "
                                                      <<imu_data.linear_acceleration.y<<" "
                                                      <<imu_data.linear_acceleration.z<<std::endl<<std::endl;
                                                      break;

          case 0x52:
            imu_data.angular_velocity.x =
                ((short)(tmpdata[3 + i] << 8 | tmpdata[2 + i])) / 32768.0 * 500;
            imu_data.angular_velocity.y =
                ((short)(tmpdata[5 + i] << 8 | tmpdata[4 + i])) / 32768.0 * 500;
            imu_data.angular_velocity.z =
                ((short)(tmpdata[7 + i] << 8 | tmpdata[6 + i])) / 32768.0 * 500;
            //ROS_INFO_STREAM("imu_data: " << imu_data);
            IMU_read_pub.publish(imu_data);

            

            std::cout<<"gyr: "<<std::setprecision(16)<<imu_data.header.stamp<<" "<<imu_data.angular_velocity.x <<" "
                                                      <<imu_data.angular_velocity.y <<" "
                                                      <<imu_data.angular_velocity.z <<std::endl;
                                                       break; 
          }
        }
      }
    }
    //     if (ser.available()) {
    //       // ROS_INFO_STREAM("Reading from serial port\n");
    //       //读到的是string类型的,
    //       std_msgs::String result;
    //       result.data = ser.read(ser.available());
    //       ROS_INFO_STREAM("Read: " << result.data);
    //       std::cout << "result.data: " << result.data << std::endl <<
    //       std::endl;

    //       // read_pub.publish(result);
    //     }

    //处理ROS的信息，比如订阅消息,并调用回调函数
    ros::spinOnce();
    loop_rate.sleep();
  }
}

CMakeLists.txt的内容

cmake_minimum_required(VERSION 2.8.3)
project(serialPort)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  serial
  std_msgs
)


catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES serialPort
  CATKIN_DEPENDS roscpp serial std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
#include
  ${catkin_INCLUDE_DIRS}
)


add_executable(serialPort src/serialPort.cpp)
 
add_dependencies(serialPort ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
target_link_libraries(serialPort
  ${catkin_LIBRARIES}
)

4. 获取uvc相机的图像

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <stdio.h>
using namespace cv;

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

  ros::init(argc, argv, "image_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);

  image_transport::Publisher pub0 = it.advertise("camera/left", 0);
  image_transport::Publisher pub1 = it.advertise("camera/right", 1);

  //测试出来我的小觅相机是单ID相机
  cv::VideoCapture cap(0);
  cap.set(CV_CAP_PROP_FRAME_WIDTH, 2560);
  cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720);
  cap.set(CV_CAP_PROP_FPS, 60);

  // cap.set(CV_CAP_PROP_FRAME_WIDTH, 1280);
  // cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
  // cap.set(CV_CAP_PROP_FPS, 60);


  if (!cap.isOpened()) {
    ROS_INFO("cannot open video device0\n");
    return -1;
  }

  cv::Mat frame, frame_G, frame_L, frame_R;

  sensor_msgs::ImagePtr msg0;
  sensor_msgs::ImagePtr msg1;
  sensor_msgs::ImagePtr msg;

  ros::Rate loop_rate(30);

  while (nh.ok()) {
    cap >> frame;
    ros::Time time_now = ros::Time::now();
    cv::cvtColor(frame, frame_G, cv::COLOR_BGR2GRAY);

    frame_L = frame_G(cv::Rect(0, 0, 1280, 720));
    frame_R = frame_G(cv::Rect(1280, 0, 1280, 720));

    // frame_L = frame_G(cv::Rect(0, 0, 640, 480));
    // frame_R = frame_G(cv::Rect(640, 0, 640, 480));

    // cv::imshow("frame_L", frame_L);
    // cv::waitKey(2);

    // cv::imshow("frame_R", frame_R);
    // cv::waitKey(2);

    // cv::imshow("frame", frame);
    // cv::waitKey(2);

    // cv::imshow("frame_G", frame_G);
    // cv::waitKey(2);

    if (!frame_L.empty()) {
      msg0 =
          cv_bridge::CvImage(std_msgs::Header(), "mono8", frame_L).toImageMsg();
      msg0->header.stamp = time_now;
      pub0.publish(msg0);
    }

    if (!frame_R.empty()) {
      msg1 =
          cv_bridge::CvImage(std_msgs::Header(), "mono8", frame_R).toImageMsg();
      msg1->header.stamp = time_now;
      pub1.publish(msg1);
    }
    ROS_INFO("Publishing camera/left camera/right ROS topic MSG!! ");

    ros::spinOnce();
    loop_rate.sleep();
  }
}

CMakeLists.txt文件内容如下

cmake_minimum_required(VERSION 2.8.3)
project(imgReader)

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

#
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES imgReader
   CATKIN_DEPENDS sensor_msgs cv_bridge roscpp std_msgs image_transport
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations

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


add_executable(imgReader src/imgReader.cpp)
 

 
target_link_libraries(imgReader
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
)
add_dependencies(imgReader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

5. 设置rviz显示图像和Imu数据

首先设置launch文件

<launch>

  <node pkg="serialPort" type="serialPort" name="serialPort" required="true" output="screen">
    <param name="port" value="/dev/ttyUSB0"/>
  </node>

  <node pkg="imgReader" type="imgReader" name="imgReader" required="true" output="screen">
    <param name="port" value="/dev/video0"/>
  </node>


<!-- 在rviz中显示-->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find imgReader)/config/rviz/get_imu_image.rviz" required="true" />

</launch>

如果不设置launch文件,也可以分别发布两个节点的信息.

terminal 1
roscore

terminal 2
source ./devel/setup.bash
rosrun serialPort serialPort

terminal 3
source ./devel/setup.bash
rosrun imgReader imgReader

参考这篇文章设置rviz的配置文件  https://blog.csdn.net/weixin_44684139/article/details/104416690 和 https://blog.csdn.net/xinmei4275/article/details/85040164?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase

首先是运行你的node,使得图像和imu数据的topics都发布出来,使用rostopic list查看当前正在发布的topic

然后打开rviz窗口界面

rosrun rviz rviz

然后添加Imu和image的topics

最后保存rviz配置到相应的路径.然后在上面的launch文件最后加上rviz配置文件,这样,当roslaunch launch文件时,就可以同时加载rviz视图窗口了.

就像下图这样.