用OpenCV编写一个ROS节点发布图像话题

1、创建功能包文件

个人习惯使用RoboWare Studio，很方便，还没入手的同学可以参考我前面的博文《RoboWare Studio安装教程》进行安装。这个IDE使用起来基本没什么难度，相信有一定ROS基础的同学上手就会。

用RoboWare打开一个你经常使用的ROS工作空间，或者你可以在终端新建一个专门实现某种功能的ROS工作空间，举个栗子，创建一个名为SLAM的工作空间：

$ mkdir -p ~/SLAM/src
$ cd ~/SLAM/src
$ catkin_init_workspace
$ cd ..
$ catkin_make
$ echo "source ~/SLAM/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc

创建完这个工作空间，用RoboWare打开就可以啦！接下来开始新建一个功能包，打开IDE中的“资源管理器”标签页，鼠标右键src文件夹，选择“新建ROS包”，编辑包名和它的依赖：my_nodes roscpp std_msgs cv_bridge image_transport OpenCV，然后回车。这里我给它起的包名叫“my_nodes”，后面的是依赖，这些依赖后期可以修改，在RoboWare上操作更方便，直接在资源管理器中右键包名选择“编辑依赖的ROS包列表”即可。

新建的ROS功能包自动生成两个文件夹（include和src）与两个配置文件（CMakeLists.txt和package.xml），而且，用RoboWare建立的功能包会自动将CMakeLists.txt和package.xml这两个文件配置好，你编辑依赖项的时候IDE会自行修改并保存，所以后面我们不需要再修改。创建程序文件的时候也一样，基本不需要自己动手修改CMakeLists.txt，这正是RoboWare使用最方便的地方。

2、编写自己的ROS节点

在RoboWare资源管理器中，鼠标右键my_nodes功能包下的src文件夹，选择“新建CPP源文件”，输入cam_node.cpp敲回车，选择“加入到新的可执行文件中”。在资源管理器中双击cam_node.cpp，开始编辑代码。下面直接贴上我的源码：

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <image_transport/image_transport.h>

#include <ros/ros.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "cam_node");

  int video_device;
  int frame_rate;
  ros::NodeHandle nh("~");
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("image_raw", 10);

  nh.param<int>("video_device", video_device, 0);
  nh.param<int>("frame_rate", frame_rate, 30);

  cv::VideoCapture TheVideoCapturer;
  TheVideoCapturer.open(video_device);
  if (!TheVideoCapturer.isOpened()) {
    ROS_WARN("Open camera device%d error!", video_device);
    return false;
  }
  ROS_INFO("Camera device%d openned, fps=%d", video_device, frame_rate);

  ros::Rate loop_rate(frame_rate);
  while (nh.ok()) {
    if (TheVideoCapturer.grab()) {
      cv::Mat image;
      TheVideoCapturer.retrieve(image);
      cv_bridge::CvImage out_msg;
      out_msg.header.stamp = ros::Time::now();
      out_msg.encoding = sensor_msgs::image_encodings::BGR8;
      out_msg.image = image;
      pub.publish(out_msg.toImageMsg());
    }
    loop_rate.sleep();
  }
  return 0;
}

代码很简单，由于时间仓促，这里就不解释了，有问题请留言。

复制粘贴上面的代码，鼠标右键my_nodes功能包将其设置为活动状态，这样的话如果你的工作空间中有多个功能包可以保证只编译当前处于活动状态的功能包。然后单击资源管理器上面的绿色小锤子，进行构建，即会在my_nodes功能包下生成一个名为cam_node的ROS节点。

3、使用launch文件配置并运行cam_node节点

我在cam_node节点中用param()函数通过节点句柄来提取参数，缺省条件下默认相机设备号为0、输出帧率为30fps。如果想要打开笔记本外接的USB相机（通常设备号为1），且设置帧率为25，则可以直接在终端运行以下命令(先确认已经运行roscore)：

$ rosrun my_nodes cam_node _video_device:=0 _frame_rate:=25

显然，如果每次都在终端输入这么一长串命令非常不方便，所以我们要使用launch文件来运行这个节点。

在RoboWare资源管理器中右键my_nodes功能包，选择“新建Launch文件夹”（如果你之前没有创建的话），然后再右键生成的launch文件夹，选择“新建LAUNCH文件”，输入一个名字（比如还叫my_nodes），敲回车。双击生成的my_nodes.launch文件，编辑ROS节点的启动脚本，复制粘贴以下代码：

<launch>
    <node name="cam_node" pkg="my_nodes" type="cam_node" output="screen" >
        <param name="video_device" value="1" />
        <param name="frame_rate" value="25" />
        
    node>
launch>

粘贴以后保存，不需要编译，然后在资源管理器右键my_nodes.launch，选择“运行Launch文件”，即可启动cam_node节点。当然，你也可以在终端运行$ rosrun my_nodes my_nodes.launch。
如果有需要，你还可以在launch文件中进行话题的重映射，将上面代码中的注释取消掉即可。你也可以映射成其他的名字，其他节点订阅的时候保证话题名称一致就OK了。

温馨提示：如果你在编译或运行过程中遇到错误，博主认为多半是OpenCV的问题，届时请参考我的这篇博客：ROS-indigo发布Mat图像以及解决cv_bridge与OpenCV3.3.1版本不兼容问题

---

参考链接

[1] https://blog.csdn.net/lixujie666/article/details/80076083
[2] https://blog.csdn.net/github_30605157/article/details/50990493
[3] https://blog.csdn.net/lixujie666/article/details/80139112