一个ROS机器视觉pipeline

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

#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("/cepth", 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;
}

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