29 #include <image_transport/image_transport.h>
30 #include <cv_bridge/cv_bridge.h>
31 #include <sensor_msgs/image_encodings.h>
32 #include <opencv2/imgproc/imgproc.hpp>
33 #include <opencv2/highgui/highgui.hpp>
35 #include <camera_info_manager/camera_info_manager.h>
38 int main(
int argc,
char** argv)
40 ros::init(argc, argv,
"usb_cam_reader");
41 ros::NodeHandle nh(
"~");
42 image_transport::ImageTransport it(nh);
43 image_transport::CameraPublisher img_pub = it.advertiseCamera(
"image_raw", 1);
46 std::string camera_info_url;
47 std::string camera_name;
49 nh.param(
"camera_name", camera_name, std::string(
"my_camera"));
50 nh.param(
"camera_info_url", camera_info_url, std::string(
""));
53 nh.param<
int>(
"cam_id",cam_id,0);
55 nh.param<
int>(
"width",img_width,0);
57 nh.param<
int>(
"height",img_height,0);
59 camera_info_manager::CameraInfoManager cinfo_manager_(nh, camera_name, camera_info_url);
65 cv::VideoCapture captura(cam_id);
68 if ( !captura.isOpened())
70 std::cout <<
"Impossible to open device" << std::endl;
73 bool resolution =
false;
74 sensor_msgs::CameraInfoPtr cam_info(
new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
80 captura.set(CV_CAP_PROP_FRAME_WIDTH, img_width);
81 captura.set(CV_CAP_PROP_FRAME_HEIGHT, img_height);
84 if ( cam_info->width != 0)
86 captura.set(CV_CAP_PROP_FRAME_WIDTH, cam_info->width);
87 captura.set(CV_CAP_PROP_FRAME_HEIGHT, cam_info->height);
93 ros::Rate loop_rate(60);
100 std::cout << frame.size() << std::endl;
103 if(img_pub.getNumSubscribers() > 0)
105 cv_bridge::CvImage my_image;
107 my_image.header.stamp = ros::Time::now();
108 my_image.header.frame_id =
"my_image";
109 my_image.encoding =
"bgr8";
110 my_image.image = frame;
113 cam_info->header.frame_id = my_image.header.frame_id;
114 cam_info->header.stamp = my_image.header.stamp;
118 img_pub.publish( my_image.toImageMsg(), cam_info);
int main(int argc, char **argv)