读取图像后分辨率设置在640x480,封装到ros节点/camera/image下一30hz发布。
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <stdio.h>
// #include <xx_msgs/Flag.h>
// static bool gainControl_flag = false;
// void gainControlCB(const xx_msgs::Flag::ConstPtr& msg)
// {
// std::string control_msg = msg->flag;
// if(control_msg == "3dlidar stop,cv start")
// {
// ROS_INFO("ImagePro gain control.");
// gainControl_flag = true;
// }
// }
//读取yaml参数
template<typename T>
T getParam(const std::string& name,const T& defaultValue) //This name must be namespace+parameter_name
{
T v;
if(ros::param::get(name,v)) //get parameter by name depend on ROS.
{
ROS_INFO_STREAM("Found parameter: "<<name<<",\tvalue: "<<v);
return v;
}
else
ROS_WARN_STREAM("Cannot find value for parameter: "<<name<<",\tassigning default: "<<defaultValue);
return defaultValue; //if the parameter haven't been set,it's value will return defaultValue.
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "camera");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("camera/image", 1);
// ros::Subscriber controlFlag_sub = nh.subscribe<xx_msgs::Flag>("flag_3dlidar_to_cv",1,gainControlCB);
// std::string video_name = getParam<std::string>("ros_cv/videoname","rtsp://admin:txz123456@192.168.1.66:554/mjpeg/ch1/sub/av_stream");
// std::string video_name = "rtsp://admin:txz123456@192.168.1.66:554/mjpeg/ch1/sub/av_stream";
// std::string video_name = "/home/xx/andyoyo/usv_ImagePro/src/ros_cv/data/lib2.avi";
//int camera_index = 0;
//cv::VideoWriter writer("/home/xx/andyoyo/data/write.avi",CV_FOURCC('M','J','P','G'),30.0,cv::Size(640,480));
cv::VideoCapture cap(1);
std::string img_name;
if(!cap.isOpened())
{
ROS_INFO("Can not open camera!");
return -1;
}
else
{
ROS_INFO("Camera opened success...");
}
cap.set(CV_CAP_PROP_FRAME_WIDTH ,640);
cap.set(CV_CAP_PROP_FRAME_HEIGHT,480);
cv::Mat frame;
ros::Rate loop_rate(30);
while(nh.ok()&&cap.isOpened())
{
// if(gainControl_flag)//控制权
{
cap>>frame;
if(frame.empty())
{
ROS_INFO("frame is empty!");
//TODO: 2018/12/26 是否该交出控制权?
return -1;
}
else
{
//writer<<frame;//保存视频
cv::imshow("camera frame",frame);
if(cv::waitKey(10)==27)
{
ROS_INFO("ImagePro ESC...");
//TODO: 2018/12/26 是否该交出控制权?
return -1;
}
//发布图像消息
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
pub.publish(msg);
}
}
ros::spinOnce();
loop_rate.sleep();
}
}