最近项目需要用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出去。