一个ROS机器视觉pipeline

之前做的项目需要用到ROS，在这方面几乎小白的我用了几天时间入了下门，又用了几天时间调bug，最终基本上实现了如下需求功能：
（1）订阅话题侦听外界的控制信息task_type，并初始化task_type=0；
（2）当task_type=1时，首先需要抓取彩色图和深度图各一张作为算法输入，然后运行算法，算法的输出结果包括一个pos数组和一张可视化图片；
（3）将算法的结果通过话题发布出去。
首先定义三个回调函数task_callback、color_callback、depth_callback，分别用来侦听控制信息、采集彩色图像、采集深度图像。在while循环中不断侦听task_type，如果未收到外界控制信息则task_type=0，程序处于等待状态；当收到外界控制信息1时，task_type=1，采集图像、运行算法并发布结果；当收到外界控制信息除0和1之外的值时，关闭所有话题结束循环退出程序。下面给出程序的main.cpp，算法的具体实现涉及项目内容不方便给出。该程序的实现思路比较简单，可以实现上述功能，但对于一些异常情况的判断还有待完善。

main.cpp

#include <stdlib.h>
#include <string>
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include "algo.h"

int task_type = 0;
cv::Mat rgb, depth, view;
float pos[24];

void task_callback(const std_msgs::Int32::ConstPtr& topic_msg)
{
  ROS_INFO("subscribing information: %d", topic_msg->data);
  task_type = topic_msg->data;
}

void color_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
  if(task_type == 1)
  {
    ros::Time time = img_msg->header.stamp;
    std::string rgb_name = std::to_string(time.sec) + "_rgb.png";
    ROS_INFO("get image: %s", rgb_name.c_str());
    rgb = cv_bridge::toCvCopy(img_msg, "8UC3")->image;
    cv::cvtColor(rgb, rgb, cv::COLOR_BGR2RGB);
    cv::imwrite(rgb_name, rgb);
  }
}

void depth_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
  if(task_type == 1)
  {
    ros::Time time = img_msg->header.stamp;
    std::string depth_name = std::to_string(time.sec) + "_depth.png";
    ROS_INFO("get image: %s", depth_name.c_str());
    depth = cv_bridge::toCvCopy(img_msg, "16UC1")->image;
    cv::imwrite(depth_name, depth);
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "algo");
  ros::NodeHandle task_nh, color_nh, depth_nh, algo_nh, view_nh;
  ros::Subscriber sub_task = task_nh.subscribe("/control", 1, task_callback);
  ros::Subscriber sub_rgb = color_nh.subscribe("/color", 1, color_callback);
  ros::Subscriber sub_depth = depth_nh.subscribe("/depth", 1, depth_callback);
  ros::Publisher pub_pose = algo_nh.advertise<std_msgs::Float32MultiArray>("/pose", 1);  
  ros::Publisher pub_view = view_nh.advertise<sensor_msgs::Image>("/view", 1);  
  ros::Rate r(1);

  while(ros::ok()) //不写ros::ok()会死循环
  {
    std::cout << "task_type: " << task_type << std::endl;

    if(task_type == 0)
    { 
      ros::spinOnce();
      r.sleep();
    }
    else if(task_type == 1)
    { 
      ros::spinOnce();
      r.sleep();

      Algo *algo = new Algo(rgb, depth);
      algo->process();

      memcpy(pos, &(algo->result_algo)[0], algo->result_algo.size() * sizeof(float));
      view = algo->view.clone();

      std_msgs::Float32MultiArray msg;
      for(int i=0;i<algo->result_algo.size();++i)
        msg.data.push_back(pos[i]);

      pub_pose.publish(msg); 

      sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", view).toImageMsg();
      pub_view.publish(img_msg); 

      task_type = 0; 
    }
    else
    {
      sub_task.shutdown();
      sub_rgb.shutdown();
      sub_depth.shutdown();
      pub_pose.shutdown();
      pub_view.shutdown();
      break;
    }
  }

  return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 

project(test)  

set(SRC_LIST main.cpp algo.cpp)
set(Boost_INCLUDE_DIR /usr/include)
set(Boost_LIBRARY_DIR /usr/lib/aarch64-linux-gnu)

find_package(OpenCV REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS roscpp)

include_directories(${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} algo.h)
link_directories(${OpenCV_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
add_definitions(${OpenCV_DEFINITIONS} ${PCL_DEFINITIONS})
add_executable(test ${SRC_LIST})

target_link_libraries(test ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${catkin_LIBRARIES}) 

发布控制信息可以通过rostopic pub命令实现，查看发布的消息可以通过rostopic echo命令实现，发布的图片可以通过rqt_image_view查看。