[ROS学习笔记]ROS在同一个节点里订阅和发布消息

最近项目需要用ROS处理GMSL摄像头传来的图像，最好是在同一个节点里处理完再发到topic上，利用Rviz代替cv::imshow()进行可视化。五星推荐原作者的文章原文地址。

//main.cpp
#include <ros/ros.h>
 
class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    //Topic you want to publish
    pub_ = n_.advertise<PUBLISHED_MESSAGE_TYPE>("/published_topic", 1);
 
    //Topic you want to subscribe
    sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this);
  }
 
  void callback(const SUBSCRIBED_MESSAGE_TYPE& input)
  {
    PUBLISHED_MESSAGE_TYPE output;
    //.... do something with the input and generate the output...
    pub_.publish(output);
  }
 
private:
  ros::NodeHandle n_; 
  ros::Publisher pub_;
  ros::Subscriber sub_;
 
}//End of class SubscribeAndPublish
 
int main(int argc, char **argv)
{
  //Initiate ROS
  ros::init(argc, argv, "subscribe_and_publish");
 
  //Create an object of class SubscribeAndPublish that will take care of everything
  SubscribeAndPublish SAPObject;
 
  ros::spin();
 
  return 0;
}

流程就是先订阅，利用回调函数对图像进行处理，处理完后publish出去。例子：

class SubscribeAndPublish
{
public:
    SubscribeAndPublish()
    {
        //pub_ = it_.advertise<PUBLISHED_MESSAGE_TYPE>("camera/image", 1);
        image_transport::ImageTransport it_(n_);
        pub_ = it_.advertise("camera/image", 1);//<sensor_msgs::Image>
        sub_ = it_.subscribe("/miivii_gmsl_ros/camera1", 1, &SubscribeAndPublish::callback, this);
    }

    void callback(const sensor_msgs::ImageConstPtr& image_source)
    {
        //PUBLISHED_MESSAGE_TYPE output;
        sensor_msgs::Image output;
        cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_source, sensor_msgs::image_encodings::BGR8);
        LaneDetector lanedetector;
        cv::Mat frame = cv_image->image.clone();
        cv::Mat img_denoise;
        cv::Mat img_edges;
        cv::Mat img_mask;
        cv::Mat img_lines;
        cv::Mat img_out;
        std::vector<cv::Vec4i> lines;
        std::vector<std::vector<cv::Vec4i> > left_right_lines;
        std::vector<cv::Point> lane;
        std::string turn;

        int flag_plot = -1;
  //int i = 0;

  // Main algorithm starts. Iterate through every frame of the video
        if (1 /*i < 540*/) {//while -> if
      // Capture frame
//      if (!cap.read(frame))
//          break;

      // Denoise the image using a Gaussian filter
            img_denoise = lanedetector.deNoise(frame);

      // Detect edges in the image
            img_edges = lanedetector.edgeDetector(img_denoise);

      // Mask the image so that we only get the ROI
            img_mask = lanedetector.mask(img_edges);

      // Obtain Hough lines in the cropped image
            lines = lanedetector.houghLines(img_mask);

            if (!lines.empty())
            {
          // Separate lines into left and right lines
                left_right_lines = lanedetector.lineSeparation(lines, img_edges);

          // Apply regression to obtain only one line for each side of the lane
                lane = lanedetector.regression(left_right_lines, frame);

          // Predict the turn by determining the vanishing point of the the lines
                turn = lanedetector.predictTurn();

          // Plot lane detection
                flag_plot = lanedetector.plotLane(frame, lane, turn, img_out/*image_pub_, it_*/);

                //image_pub_ = it_.advertise("camera/image_lane", 1);
                //ros::Rate loop_rate(20);
                sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_out).toImageMsg();
                pub_.publish(msg);
                //image_pub_.publish(msg);
                //ros::spinOnce();
                //i += 1;

            }
            else
            {
                flag_plot = -1;
            }
        }

        //pub_.publish(msg);
    }

private:
    ros::NodeHandle n_;
    //ros::Publisher pub_;
    //image_transport::ImageTransport it_(n_);
    image_transport::Publisher pub_;
    //ros::Subscriber sub_;
    image_transport::Subscriber sub_;
};

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "line_ocv");
    SubscribeAndPublish SAPObject;
    ros::spin();

    return 0;
}

流程就是先订阅，利用回调函数对图像进行处理，处理完后publish出去。