From 055e6236d818279684088b2f716f138ddedd9bd7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 20 Apr 2019 06:55:00 +0000 Subject: [PATCH] add queue_size parameter to all nodes, see #83 --- src/nodelet/adding_images_nodelet.cpp | 10 ++++++---- src/nodelet/camshift_nodelet.cpp | 6 ++++-- src/nodelet/color_filter_nodelet.cpp | 6 ++++-- src/nodelet/contour_moments_nodelet.cpp | 6 ++++-- src/nodelet/convex_hull_nodelet.cpp | 6 ++++-- src/nodelet/corner_harris_nodelet.cpp | 6 ++++-- src/nodelet/discrete_fourier_transform_nodelet.cpp | 6 ++++-- src/nodelet/edge_detection_nodelet.cpp | 6 ++++-- src/nodelet/face_detection_nodelet.cpp | 6 ++++-- src/nodelet/find_contours_nodelet.cpp | 6 ++++-- src/nodelet/general_contours_nodelet.cpp | 6 ++++-- src/nodelet/goodfeature_track_nodelet.cpp | 6 ++++-- src/nodelet/hough_circles_nodelet.cpp | 6 ++++-- src/nodelet/hough_lines_nodelet.cpp | 6 ++++-- src/nodelet/lk_flow_nodelet.cpp | 6 ++++-- src/nodelet/people_detect_nodelet.cpp | 6 ++++-- src/nodelet/phase_corr_nodelet.cpp | 6 ++++-- src/nodelet/pyramids_nodelet.cpp | 6 ++++-- src/nodelet/segment_objects_nodelet.cpp | 6 ++++-- src/nodelet/simple_compressed_example_nodelet.cpp | 4 +++- src/nodelet/simple_example_nodelet.cpp | 4 +++- src/nodelet/simple_flow_nodelet.cpp | 6 ++++-- src/nodelet/smoothing_nodelet.cpp | 6 ++++-- src/nodelet/threshold_nodelet.cpp | 6 ++++-- src/nodelet/watershed_segmentation_nodelet.cpp | 6 ++++-- 25 files changed, 100 insertions(+), 50 deletions(-) diff --git a/src/nodelet/adding_images_nodelet.cpp b/src/nodelet/adding_images_nodelet.cpp index e2f5cddd..514f8b63 100644 --- a/src/nodelet/adding_images_nodelet.cpp +++ b/src/nodelet/adding_images_nodelet.cpp @@ -77,6 +77,7 @@ namespace opencv_apps { Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -116,13 +117,13 @@ namespace opencv_apps { if (config_.use_camera_info) { if (approximate_sync_) { async_with_info_ = boost::make_shared< - message_filters::Synchronizer >(100); + message_filters::Synchronizer >(queue_size_); async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); async_with_info_->registerCallback(boost::bind( &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); } else { sync_with_info_ = - boost::make_shared >(100); + boost::make_shared >(queue_size_); sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); sync_with_info_->registerCallback(boost::bind( &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); @@ -130,13 +131,13 @@ namespace opencv_apps { } else { if (approximate_sync_) { async_ = boost::make_shared< - message_filters::Synchronizer >(100); + message_filters::Synchronizer >(queue_size_); async_->connectInput(sub_image1_, sub_image2_); async_->registerCallback( boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); } else { sync_ = - boost::make_shared >(100); + boost::make_shared >(queue_size_); sync_->connectInput(sub_image1_, sub_image2_); sync_->registerCallback( boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); @@ -251,6 +252,7 @@ namespace opencv_apps { reconfigure_server_->setCallback(f); pnh_->param("approximate_sync", approximate_sync_, true); + pnh_->param("queue_size", queue_size_, 100); img_pub_ = advertiseImage(*pnh_, "image", 1); onInitPostProcess(); } diff --git a/src/nodelet/camshift_nodelet.cpp b/src/nodelet/camshift_nodelet.cpp index 0b88ddb5..494ba306 100644 --- a/src/nodelet/camshift_nodelet.cpp +++ b/src/nodelet/camshift_nodelet.cpp @@ -71,6 +71,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -337,9 +338,9 @@ class CamShiftNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &CamShiftNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &CamShiftNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &CamShiftNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &CamShiftNodelet::imageCallback, this); } void unsubscribe() @@ -355,6 +356,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp index 891c9328..74950aea 100644 --- a/src/nodelet/color_filter_nodelet.cpp +++ b/src/nodelet/color_filter_nodelet.cpp @@ -74,6 +74,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; std::string window_name_; @@ -145,9 +146,9 @@ class ColorFilterNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &ColorFilterNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &ColorFilterNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &ColorFilterNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &ColorFilterNodelet::imageCallback, this); } void unsubscribe() @@ -163,6 +164,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { diff --git a/src/nodelet/contour_moments_nodelet.cpp b/src/nodelet/contour_moments_nodelet.cpp index a8d463b1..482f93c2 100644 --- a/src/nodelet/contour_moments_nodelet.cpp +++ b/src/nodelet/contour_moments_nodelet.cpp @@ -78,6 +78,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -238,9 +239,9 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &ContourMomentsNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &ContourMomentsNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &ContourMomentsNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &ContourMomentsNodelet::imageCallback, this); } void unsubscribe() @@ -256,6 +257,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/convex_hull_nodelet.cpp b/src/nodelet/convex_hull_nodelet.cpp index 2a1b8ec9..56f3f2d0 100644 --- a/src/nodelet/convex_hull_nodelet.cpp +++ b/src/nodelet/convex_hull_nodelet.cpp @@ -70,6 +70,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -199,9 +200,9 @@ class ConvexHullNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &ConvexHullNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &ConvexHullNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &ConvexHullNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &ConvexHullNodelet::imageCallback, this); } void unsubscribe() @@ -218,6 +219,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/corner_harris_nodelet.cpp b/src/nodelet/corner_harris_nodelet.cpp index d881648b..a18eb3f8 100644 --- a/src/nodelet/corner_harris_nodelet.cpp +++ b/src/nodelet/corner_harris_nodelet.cpp @@ -66,6 +66,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; std::string window_name_; @@ -174,9 +175,9 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &CornerHarrisNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &CornerHarrisNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &CornerHarrisNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &CornerHarrisNodelet::imageCallback, this); } void unsubscribe() @@ -192,6 +193,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { diff --git a/src/nodelet/discrete_fourier_transform_nodelet.cpp b/src/nodelet/discrete_fourier_transform_nodelet.cpp index 740f70a4..1dcd62fb 100644 --- a/src/nodelet/discrete_fourier_transform_nodelet.cpp +++ b/src/nodelet/discrete_fourier_transform_nodelet.cpp @@ -63,6 +63,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -84,10 +85,10 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera( - "image", 1, &DiscreteFourierTransformNodelet::imageCallbackWithInfo, this); + "image", queue_size_, &DiscreteFourierTransformNodelet::imageCallbackWithInfo, this); else img_sub_ = - it_->subscribe("image", 1, &DiscreteFourierTransformNodelet::imageCallback, this); + it_->subscribe("image", queue_size_, &DiscreteFourierTransformNodelet::imageCallback, this); } void unsubscribe() { @@ -178,6 +179,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 1); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { diff --git a/src/nodelet/edge_detection_nodelet.cpp b/src/nodelet/edge_detection_nodelet.cpp index 7bb3eb93..ceb9864b 100644 --- a/src/nodelet/edge_detection_nodelet.cpp +++ b/src/nodelet/edge_detection_nodelet.cpp @@ -77,6 +77,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -261,9 +262,9 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &EdgeDetectionNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &EdgeDetectionNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &EdgeDetectionNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &EdgeDetectionNodelet::imageCallback, this); } void unsubscribe() @@ -279,6 +280,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { diff --git a/src/nodelet/face_detection_nodelet.cpp b/src/nodelet/face_detection_nodelet.cpp index faff76ca..73c93a17 100644 --- a/src/nodelet/face_detection_nodelet.cpp +++ b/src/nodelet/face_detection_nodelet.cpp @@ -71,6 +71,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -202,9 +203,9 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &FaceDetectionNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &FaceDetectionNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &FaceDetectionNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &FaceDetectionNodelet::imageCallback, this); } void unsubscribe() @@ -220,6 +221,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_ ) { always_subscribe_ = true; diff --git a/src/nodelet/find_contours_nodelet.cpp b/src/nodelet/find_contours_nodelet.cpp index 99dcbef9..52f0ba4b 100644 --- a/src/nodelet/find_contours_nodelet.cpp +++ b/src/nodelet/find_contours_nodelet.cpp @@ -70,6 +70,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -196,9 +197,9 @@ class FindContoursNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &FindContoursNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &FindContoursNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &FindContoursNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &FindContoursNodelet::imageCallback, this); } void unsubscribe() @@ -214,6 +215,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/general_contours_nodelet.cpp b/src/nodelet/general_contours_nodelet.cpp index 70ffa4bb..2830bace 100644 --- a/src/nodelet/general_contours_nodelet.cpp +++ b/src/nodelet/general_contours_nodelet.cpp @@ -70,6 +70,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -227,9 +228,9 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &GeneralContoursNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &GeneralContoursNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &GeneralContoursNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &GeneralContoursNodelet::imageCallback, this); } void unsubscribe() @@ -245,6 +246,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/goodfeature_track_nodelet.cpp b/src/nodelet/goodfeature_track_nodelet.cpp index 00e33970..79a1aa43 100644 --- a/src/nodelet/goodfeature_track_nodelet.cpp +++ b/src/nodelet/goodfeature_track_nodelet.cpp @@ -68,6 +68,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -199,9 +200,9 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &GoodfeatureTrackNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &GoodfeatureTrackNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &GoodfeatureTrackNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &GoodfeatureTrackNodelet::imageCallback, this); } void unsubscribe() @@ -217,6 +218,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/hough_circles_nodelet.cpp b/src/nodelet/hough_circles_nodelet.cpp index 48b4f32a..9b1eab67 100644 --- a/src/nodelet/hough_circles_nodelet.cpp +++ b/src/nodelet/hough_circles_nodelet.cpp @@ -70,6 +70,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -294,9 +295,9 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &HoughCirclesNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &HoughCirclesNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &HoughCirclesNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &HoughCirclesNodelet::imageCallback, this); } void unsubscribe() @@ -313,6 +314,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); debug_image_type_ = 0; + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = debug_view_; diff --git a/src/nodelet/hough_lines_nodelet.cpp b/src/nodelet/hough_lines_nodelet.cpp index e94ef2ae..fb203a95 100644 --- a/src/nodelet/hough_lines_nodelet.cpp +++ b/src/nodelet/hough_lines_nodelet.cpp @@ -70,6 +70,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -260,9 +261,9 @@ class HoughLinesNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &HoughLinesNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &HoughLinesNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &HoughLinesNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &HoughLinesNodelet::imageCallback, this); } void unsubscribe() @@ -278,6 +279,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/lk_flow_nodelet.cpp b/src/nodelet/lk_flow_nodelet.cpp index 6d7e0865..8e893225 100644 --- a/src/nodelet/lk_flow_nodelet.cpp +++ b/src/nodelet/lk_flow_nodelet.cpp @@ -71,6 +71,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -288,9 +289,9 @@ class LKFlowNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &LKFlowNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &LKFlowNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &LKFlowNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &LKFlowNodelet::imageCallback, this); } void unsubscribe() @@ -306,6 +307,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/people_detect_nodelet.cpp b/src/nodelet/people_detect_nodelet.cpp index e1af18fb..fc0f0f05 100644 --- a/src/nodelet/people_detect_nodelet.cpp +++ b/src/nodelet/people_detect_nodelet.cpp @@ -68,6 +68,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -191,9 +192,9 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &PeopleDetectNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &PeopleDetectNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &PeopleDetectNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &PeopleDetectNodelet::imageCallback, this); } void unsubscribe() @@ -209,6 +210,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/phase_corr_nodelet.cpp b/src/nodelet/phase_corr_nodelet.cpp index 4843b9a2..0cc4f8a9 100644 --- a/src/nodelet/phase_corr_nodelet.cpp +++ b/src/nodelet/phase_corr_nodelet.cpp @@ -62,6 +62,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -179,9 +180,9 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &PhaseCorrNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &PhaseCorrNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &PhaseCorrNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &PhaseCorrNodelet::imageCallback, this); } void unsubscribe() @@ -197,6 +198,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/pyramids_nodelet.cpp b/src/nodelet/pyramids_nodelet.cpp index 9840fd06..65c1e7f7 100644 --- a/src/nodelet/pyramids_nodelet.cpp +++ b/src/nodelet/pyramids_nodelet.cpp @@ -67,6 +67,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet { Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; int num_of_pyramids_; @@ -144,9 +145,9 @@ class PyramidsNodelet : public opencv_apps::Nodelet { { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &PyramidsNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &PyramidsNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &PyramidsNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &PyramidsNodelet::imageCallback, this); } void unsubscribe() @@ -161,6 +162,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { diff --git a/src/nodelet/segment_objects_nodelet.cpp b/src/nodelet/segment_objects_nodelet.cpp index ba7c19b8..d3fabd3e 100644 --- a/src/nodelet/segment_objects_nodelet.cpp +++ b/src/nodelet/segment_objects_nodelet.cpp @@ -71,6 +71,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -227,9 +228,9 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &SegmentObjectsNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &SegmentObjectsNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &SegmentObjectsNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &SegmentObjectsNodelet::imageCallback, this); } void unsubscribe() @@ -245,6 +246,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/simple_compressed_example_nodelet.cpp b/src/nodelet/simple_compressed_example_nodelet.cpp index 6dffda89..1ea44c4d 100644 --- a/src/nodelet/simple_compressed_example_nodelet.cpp +++ b/src/nodelet/simple_compressed_example_nodelet.cpp @@ -62,17 +62,19 @@ class ImageConverter ros::NodeHandle nh_; ros::Subscriber image_sub_; ros::Publisher image_pub_; + int queue_size_; bool debug_view_; public: ImageConverter() { // Subscrive to input video feed and publish output video feed - image_sub_ = nh_.subscribe("image/compressed", 1, + image_sub_ = nh_.subscribe("image/compressed", queue_size_, &ImageConverter::imageCb,this); image_pub_ = nh_.advertise("/image_converter/output_video/compressed", 1); ros::NodeHandle pnh_("~"); + pnh_.param("queue_size", queue_size_, 3); pnh_.param("debug_view", debug_view_, false); if( debug_view_) { cv::namedWindow(OPENCV_WINDOW); diff --git a/src/nodelet/simple_example_nodelet.cpp b/src/nodelet/simple_example_nodelet.cpp index fe85e37f..4434ecdc 100644 --- a/src/nodelet/simple_example_nodelet.cpp +++ b/src/nodelet/simple_example_nodelet.cpp @@ -60,6 +60,7 @@ class ImageConverter image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; image_transport::Publisher image_pub_; + int queue_size_; bool debug_view_; public: @@ -67,11 +68,12 @@ class ImageConverter : it_(nh_) { // Subscrive to input video feed and publish output video feed - image_sub_ = it_.subscribe("image", 1, + image_sub_ = it_.subscribe("image", queue_size_, &ImageConverter::imageCb, this); image_pub_ = it_.advertise("/image_converter/output_video/raw", 1); ros::NodeHandle pnh_("~"); + pnh_.param("queue_size", queue_size_, 1); pnh_.param("debug_view", debug_view_, false); if( debug_view_) { cv::namedWindow(OPENCV_WINDOW); diff --git a/src/nodelet/simple_flow_nodelet.cpp b/src/nodelet/simple_flow_nodelet.cpp index 0594f557..bed52cf3 100644 --- a/src/nodelet/simple_flow_nodelet.cpp +++ b/src/nodelet/simple_flow_nodelet.cpp @@ -69,6 +69,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; int subscriber_count_; ros::Time prev_stamp_; @@ -215,9 +216,9 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &SimpleFlowNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &SimpleFlowNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &SimpleFlowNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &SimpleFlowNodelet::imageCallback, this); } void unsubscribe() @@ -233,6 +234,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/smoothing_nodelet.cpp b/src/nodelet/smoothing_nodelet.cpp index 84115809..f9ef7dd5 100644 --- a/src/nodelet/smoothing_nodelet.cpp +++ b/src/nodelet/smoothing_nodelet.cpp @@ -74,6 +74,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -188,9 +189,9 @@ class SmoothingNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &SmoothingNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &SmoothingNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &SmoothingNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &SmoothingNodelet::imageCallback, this); } void unsubscribe() @@ -206,6 +207,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/threshold_nodelet.cpp b/src/nodelet/threshold_nodelet.cpp index 6593153d..b400c001 100644 --- a/src/nodelet/threshold_nodelet.cpp +++ b/src/nodelet/threshold_nodelet.cpp @@ -61,6 +61,7 @@ namespace opencv_apps { Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; std::string window_name_; @@ -91,10 +92,10 @@ namespace opencv_apps { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera( - "image", 1, &ThresholdNodelet::imageCallbackWithInfo, this); + "image", queue_size_, &ThresholdNodelet::imageCallbackWithInfo, this); else img_sub_ = - it_->subscribe("image", 1, &ThresholdNodelet::imageCallback, this); + it_->subscribe("image", queue_size_, &ThresholdNodelet::imageCallback, this); } void unsubscribe() { @@ -148,6 +149,7 @@ namespace opencv_apps { it_ = boost::shared_ptr( new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; diff --git a/src/nodelet/watershed_segmentation_nodelet.cpp b/src/nodelet/watershed_segmentation_nodelet.cpp index 787d5f15..3b542f76 100644 --- a/src/nodelet/watershed_segmentation_nodelet.cpp +++ b/src/nodelet/watershed_segmentation_nodelet.cpp @@ -69,6 +69,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -281,9 +282,9 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera("image", 3, &WatershedSegmentationNodelet::imageCallbackWithInfo, this); + cam_sub_ = it_->subscribeCamera("image", queue_size_, &WatershedSegmentationNodelet::imageCallbackWithInfo, this); else - img_sub_ = it_->subscribe("image", 3, &WatershedSegmentationNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &WatershedSegmentationNodelet::imageCallback, this); } void unsubscribe() @@ -299,6 +300,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true;