39 #include <dynamic_reconfigure/server.h> 42 #include <opencv2/highgui/highgui.hpp> 43 #include <opencv2/imgproc/imgproc.hpp> 47 #include <boost/filesystem.hpp> 48 #include <boost/assign/list_of.hpp> 49 #include <boost/thread/thread.hpp> 52 #include <video_stream_opencv/VideoStreamConfig.h> 54 namespace fs = boost::filesystem;
80 cam_info_msg.header.frame_id = img->header.frame_id;
82 cam_info_msg.height = img->height;
83 cam_info_msg.width = img->width;
87 cam_info_msg.distortion_model =
"plumb_bob";
89 cam_info_msg.D.resize(5, 0.0);
91 cam_info_msg.K = boost::assign::list_of(img->width/2.0) (0.0) (img->width/2.0)
92 (0.0) (img->height/2.0) (img->height/2.0)
95 cam_info_msg.R = boost::assign::list_of (1.0) (0.0) (0.0)
99 cam_info_msg.P = boost::assign::list_of (img->width/2.0) (0.0) (img->width/2.0) (0.0)
100 (0.0) (img->height/2.0) (img->height/2.0) (0.0)
101 (0.0) (0.0) (1.0) (0.0);
109 VideoStreamConfig latest_config =
config;
110 ros::Rate camera_fps_rate(latest_config.set_camera_fps);
112 int frame_counter = 0;
114 capture_thread_running =
true;
115 while (nh->ok() && capture_thread_running && subscriber_num > 0) {
117 std::lock_guard<std::mutex> lock(c_mutex);
120 if (!cap->isOpened()) {
125 if (!cap->read(frame)) {
127 if (latest_config.reopen_on_read_failure) {
135 if (video_stream_provider_type ==
"videofile")
137 camera_fps_rate.
sleep();
139 NODELET_DEBUG_STREAM(
"Current frame_counter: " << frame_counter <<
" latest_config.stop_frame - latest_config.start_frame: " << latest_config.stop_frame - latest_config.start_frame);
140 if (video_stream_provider_type ==
"videofile" &&
141 frame_counter >= latest_config.stop_frame - latest_config.start_frame)
143 if (latest_config.loop_videofile)
145 cap->open(video_stream_provider);
146 cap->set(cv::CAP_PROP_POS_FRAMES, latest_config.start_frame);
157 std::lock_guard<std::mutex> g(q_mutex);
159 while (framesQueue.size() >= latest_config.buffer_queue_size) {
162 framesQueue.push(frame.clone());
169 bool is_new_image =
false;
170 sensor_msgs::ImagePtr msg;
171 std_msgs::Header header;
172 VideoStreamConfig latest_config;
175 std::lock_guard<std::mutex> lock(p_mutex);
179 header.frame_id = latest_config.frame_id;
181 std::lock_guard<std::mutex> g(q_mutex);
182 if (!framesQueue.empty()){
183 frame = framesQueue.front();
190 if(!frame.empty() && is_new_image) {
194 if (latest_config.flip_horizontal && latest_config.flip_vertical)
195 cv::flip(frame, frame, -1);
196 else if (latest_config.flip_horizontal)
197 cv::flip(frame, frame, 1);
198 else if (latest_config.flip_vertical)
199 cv::flip(frame, frame, 0);
201 boost::make_shared<cv_bridge::CvImage>(header,
"bgr8",
frame);
202 if (latest_config.output_encoding !=
"bgr8")
207 }
catch (std::runtime_error &ex) {
209 <<
": " << ex.what());
212 msg = cv_image->toImageMsg();
214 if (cam_info_msg.distortion_model ==
""){
215 NODELET_WARN_STREAM(
"No calibration file given, publishing a reasonable default camera info.");
225 VideoStreamConfig& latest_config =
config;
228 *nh, latest_config.camera_name, latest_config.camera_info_url);
231 cam_info_msg.header.frame_id = latest_config.frame_id;
234 cap.reset(
new cv::VideoCapture);
236 int device_num = std::stoi(video_stream_provider);
238 cap->open(device_num);
239 }
catch (std::invalid_argument &ex) {
241 cap->open(video_stream_provider);
242 if(video_stream_provider_type ==
"videofile" )
246 if(latest_config.stop_frame == -1)
248 NODELET_WARN_STREAM(
"'stop_frame' set to -1, setting internally (won't be shown in dynamic_reconfigure) to last frame: " << cap->get(cv::CAP_PROP_FRAME_COUNT));
249 latest_config.stop_frame = cap->get(cv::CAP_PROP_FRAME_COUNT);
251 if(latest_config.stop_frame > cap->get(cv::CAP_PROP_FRAME_COUNT))
253 NODELET_ERROR_STREAM(
"Invalid 'stop_frame' " << latest_config.stop_frame <<
" for video which has " << cap->get(cv::CAP_PROP_FRAME_COUNT) <<
" frames. Setting internally (won't be shown in dynamic_reconfigure) 'stop_frame' to " << cap->get(cv::CAP_PROP_FRAME_COUNT));
254 latest_config.stop_frame = cap->get(cv::CAP_PROP_FRAME_COUNT);
257 if(latest_config.start_frame >= latest_config.stop_frame)
259 NODELET_ERROR_STREAM(
"Invalid 'start_frame' " << latest_config.start_frame <<
", which exceeds 'stop_frame' " << latest_config.stop_frame <<
". Setting internally (won't be shown in dynamic_reconfigure) 'start_frame' to 0.");
260 latest_config.start_frame = 0;
263 cap->set(cv::CAP_PROP_POS_FRAMES, latest_config.start_frame);
265 if (!cap->isOpened()) {
270 NODELET_INFO_STREAM(
"Video stream provider type detected: " << video_stream_provider_type);
272 double reported_camera_fps;
275 reported_camera_fps = cap->get(cv::CAP_PROP_FPS);
276 if (reported_camera_fps > 0.0)
281 cap->set(cv::CAP_PROP_FPS, latest_config.set_camera_fps);
282 if(!cap->isOpened()){
286 if (latest_config.width != 0 && latest_config.height != 0){
287 cap->set(cv::CAP_PROP_FRAME_WIDTH, latest_config.width);
288 cap->set(cv::CAP_PROP_FRAME_HEIGHT, latest_config.height);
291 cap->set(cv::CAP_PROP_BRIGHTNESS, latest_config.brightness);
292 cap->set(cv::CAP_PROP_CONTRAST, latest_config.contrast);
293 cap->set(cv::CAP_PROP_HUE, latest_config.hue);
294 cap->set(cv::CAP_PROP_SATURATION, latest_config.saturation);
296 if (latest_config.auto_exposure) {
297 cap->set(cv::CAP_PROP_AUTO_EXPOSURE, 0.75);
298 latest_config.exposure = 0.5;
300 cap->set(cv::CAP_PROP_AUTO_EXPOSURE, 0.25);
301 cap->set(cv::CAP_PROP_EXPOSURE, latest_config.exposure);
305 capture_thread = boost::thread(
307 publish_timer = nh->createTimer(
309 }
catch (std::exception& e) {
316 publish_timer.
stop();
317 capture_thread_running =
false;
318 capture_thread.join();
323 std::lock_guard<std::mutex> lock(s_mutex);
325 if (subscriber_num == 1) {
331 std::lock_guard<std::mutex> lock(s_mutex);
332 bool always_subscribe =
false;
333 pnh->getParamCached(
"always_subscribe", always_subscribe);
334 if (video_stream_provider ==
"videofile" || always_subscribe) {
339 if (subscriber_num == 0) {
363 if (new_config.fps > new_config.set_camera_fps) {
365 "Asked to publish at 'fps' (" << new_config.fps
366 <<
") which is higher than the 'set_camera_fps' (" << new_config.set_camera_fps <<
367 "), we can't publish faster than the camera provides images.");
368 new_config.fps = new_config.set_camera_fps;
372 std::lock_guard<std::mutex> c_lock(c_mutex);
373 std::lock_guard<std::mutex> p_lock(p_mutex);
383 NODELET_INFO_STREAM(
"Setting buffer size for capturing frames to: " << new_config.buffer_queue_size);
384 NODELET_INFO_STREAM(
"Flip horizontal image is: " << ((new_config.flip_horizontal)?
"true":
"false"));
385 NODELET_INFO_STREAM(
"Flip vertical image is: " << ((new_config.flip_vertical)?
"true":
"false"));
389 if (new_config.width != 0 && new_config.height != 0)
396 if (subscriber_num > 0 && (level & 0x1))
398 NODELET_DEBUG(
"New dynamic_reconfigure config received on a parameter with configure level 1, unsubscribing and subscribing");
413 int device_num = std::stoi(video_stream_provider);
414 video_stream_provider_type =
"videodevice";
415 }
catch (std::invalid_argument &ex) {
416 if (video_stream_provider.find(
"http://") != std::string::npos ||
417 video_stream_provider.find(
"https://") != std::string::npos){
418 video_stream_provider_type =
"http_stream";
420 else if(video_stream_provider.find(
"rtsp://") != std::string::npos){
421 video_stream_provider_type =
"rtsp_stream";
424 fs::file_type file_type = fs::status(fs::canonical(fs::path(video_stream_provider))).type();
425 if(fs::path(video_stream_provider) != fs::canonical(fs::path(video_stream_provider)))
426 NODELET_WARN_STREAM(
"Video stream provider path converted from: '" << fs::path(video_stream_provider) <<
427 "' to its canonical path: '" << fs::canonical(fs::path(video_stream_provider)) <<
"'" );
429 case fs::file_type::character_file:
430 case fs::file_type::block_file:
431 video_stream_provider_type =
"videodevice";
433 case fs::file_type::regular_file:
434 video_stream_provider_type =
"videofile";
437 video_stream_provider_type =
"unknown";
443 dyn_srv = boost::make_shared<dynamic_reconfigure::Server<VideoStreamConfig> >(*pnh);
445 dyn_srv->setCallback(
f);
458 connect_cb, disconnect_cb,
459 info_connect_cb, info_disconnect_cb,
464 if (subscriber_num > 0)
#define NODELET_ERROR_STREAM_THROTTLE(rate,...)
#define NODELET_INFO_STREAM(...)
virtual void disconnectionCallback(const image_transport::SingleSubscriberPublisher &)
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define NODELET_WARN_STREAM_THROTTLE(rate,...)
sensor_msgs::CameraInfo getCameraInfo(void)
#define NODELET_WARN(...)
virtual ~VideoStreamNodelet()
boost::thread capture_thread
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define NODELET_WARN_STREAM(...)
image_transport::CameraPublisher pub
virtual void do_capture()
virtual void unsubscribe()
virtual void configCallback(VideoStreamConfig &new_config, uint32_t level)
ros::NodeHandle & getPrivateNodeHandle() const
std::string video_stream_provider_type
#define NODELET_ERROR_STREAM(...)
boost::shared_ptr< ros::NodeHandle > nh
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
virtual void connectionCallbackImpl()
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
virtual void disconnectionCallbackImpl()
boost::shared_ptr< dynamic_reconfigure::Server< VideoStreamConfig > > dyn_srv
virtual sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr img)
boost::shared_ptr< cv::VideoCapture > cap
ros::NodeHandle & getNodeHandle() const
#define NODELET_DEBUG_STREAM(...)
virtual void do_publish(const ros::TimerEvent &event)
virtual void infoDisconnectionCallback(const ros::SingleSubscriberPublisher &)
std::string video_stream_provider
virtual void connectionCallback(const image_transport::SingleSubscriberPublisher &)
#define NODELET_INFO(...)
std::queue< cv::Mat > framesQueue
sensor_msgs::CameraInfo cam_info_msg
#define NODELET_FATAL_STREAM(...)
bool capture_thread_running
boost::shared_ptr< ros::NodeHandle > pnh
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void infoConnectionCallback(const ros::SingleSubscriberPublisher &)
#define NODELET_DEBUG(...)