From 5f1786bcea1e3305c2b3b38edfedb5927bce6b02 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 18 Mar 2020 21:55:24 +0900 Subject: [PATCH 1/3] add dynamic reconfigure which is removed in #479 --- image_view/cfg/ImageView.cfg | 2 +- image_view/src/nodelets/image_nodelet.cpp | 41 ++++++++++++++++++++++- 2 files changed, 41 insertions(+), 2 deletions(-) diff --git a/image_view/cfg/ImageView.cfg b/image_view/cfg/ImageView.cfg index 8cc088835..741911474 100755 --- a/image_view/cfg/ImageView.cfg +++ b/image_view/cfg/ImageView.cfg @@ -21,7 +21,7 @@ edit_method_colormap = gen.enum([ gen.const("HOT", int_t, 11, "COLORMAP_HOT"), ], "colormap") -gen.add('do_dynamic_scaling', bool_t, 0, 'Do dynamic scaling about pixel values or not', False) +gen.add('do_dynamic_scaling', bool_t, 0, 'Do dynamic scaling about pixel values or not', True) gen.add('colormap', int_t, 0, "colormap", -1, -1, 11, edit_method=edit_method_colormap); gen.add('min_image_value', double_t, 0, "Minimum image value for scaling depth/float image.", default=0, min=0); gen.add('max_image_value', double_t, 0, "Maximum image value for scaling depth/float image.", default=0, min=0); diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp index 13483b2f3..db498279a 100644 --- a/image_view/src/nodelets/image_nodelet.cpp +++ b/image_view/src/nodelets/image_nodelet.cpp @@ -31,9 +31,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include + #include #include #include +#include #include #include @@ -100,9 +103,17 @@ class ImageNodelet : public nodelet::Nodelet bool autosize_; boost::format filename_format_; int count_; + + dynamic_reconfigure::Server srv_; + bool do_dynamic_scaling_; + int colormap_; + double min_image_value_; + double max_image_value_; virtual void onInit(); + void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level); + void imageCb(const sensor_msgs::ImageConstPtr& msg); static void mouseCb(int event, int x, int y, int flags, void* param); @@ -167,17 +178,45 @@ void ImageNodelet::onInit() image_transport::ImageTransport it(nh); image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle()); sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints); + + dynamic_reconfigure::Server::CallbackType f = + boost::bind(&ImageNodelet::reconfigureCb, this, _1, _2); + srv_.setCallback(f); +} + +void ImageNodelet::reconfigureCb(image_view::ImageViewConfig &config, uint32_t level) +{ + do_dynamic_scaling_ = config.do_dynamic_scaling; + colormap_ = config.colormap; + min_image_value_ = config.min_image_value; + max_image_value_ = config.max_image_value; } void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) { // We want to scale floating point images so that they display nicely - bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos); + bool do_dynamic_scaling = do_dynamic_scaling_ & (msg->encoding.find("F") != std::string::npos); // Convert to OpenCV native BGR color try { cv_bridge::CvtColorForDisplayOptions options; options.do_dynamic_scaling = do_dynamic_scaling; + options.colormap = colormap_; + // Set min/max value for scaling to visualize depth/float image. + if (min_image_value_ == max_image_value_) { + // Not specified by rosparam, then set default value. + // Because of current sensor limitation, we use 10m as default of max range of depth + // with consistency to the configuration in rqt_image_view. + options.min_image_value = 0; + if (msg->encoding == "32FC1") { + options.max_image_value = 10; // 10 [m] + } else if (msg->encoding == "16UC1") { + options.max_image_value = 10 * 1000; // 10 * 1000 [mm] + } + } else { + options.min_image_value = min_image_value_; + options.max_image_value = max_image_value_; + } queued_image_.set(cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image); } catch (cv_bridge::Exception& e) { From 97fb651eb5032449a917aaf608ac5f8d59f20f23 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 18 Mar 2020 22:25:38 +0900 Subject: [PATCH 2/3] publish converted image as rostopic --- image_view/src/nodelets/image_nodelet.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp index db498279a..ec127a22e 100644 --- a/image_view/src/nodelets/image_nodelet.cpp +++ b/image_view/src/nodelets/image_nodelet.cpp @@ -104,6 +104,8 @@ class ImageNodelet : public nodelet::Nodelet boost::format filename_format_; int count_; + ros::Publisher pub_; + dynamic_reconfigure::Server srv_; bool do_dynamic_scaling_; int colormap_; @@ -178,6 +180,7 @@ void ImageNodelet::onInit() image_transport::ImageTransport it(nh); image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle()); sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints); + pub_ = local_nh.advertise("output", 1); dynamic_reconfigure::Server::CallbackType f = boost::bind(&ImageNodelet::reconfigureCb, this, _1, _2); @@ -198,6 +201,7 @@ void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) bool do_dynamic_scaling = do_dynamic_scaling_ & (msg->encoding.find("F") != std::string::npos); // Convert to OpenCV native BGR color + cv_bridge::CvImageConstPtr cv_ptr; try { cv_bridge::CvtColorForDisplayOptions options; options.do_dynamic_scaling = do_dynamic_scaling; @@ -217,12 +221,16 @@ void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) options.min_image_value = min_image_value_; options.max_image_value = max_image_value_; } - queued_image_.set(cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image); + cv_ptr = cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options); + queued_image_.set(cv_ptr->image); } catch (cv_bridge::Exception& e) { NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'", msg->encoding.c_str(), e.what()); } + if (pub_.getNumSubscribers() > 0) { + pub_.publish(cv_ptr); + } } void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param) @@ -286,6 +294,8 @@ void ImageNodelet::windowThread() cv::destroyWindow(window_name_); + pub_.shutdown(); + if (ros::ok()) { ros::shutdown(); From c7a920d31465fc261deab6e79acd7709a8119593 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 19 Mar 2020 11:38:01 +0900 Subject: [PATCH 3/3] use do_dynamic_scaling in dynamic reconfigure only for non-float images --- image_view/cfg/ImageView.cfg | 2 +- image_view/src/nodelets/image_nodelet.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/image_view/cfg/ImageView.cfg b/image_view/cfg/ImageView.cfg index 741911474..8cc088835 100755 --- a/image_view/cfg/ImageView.cfg +++ b/image_view/cfg/ImageView.cfg @@ -21,7 +21,7 @@ edit_method_colormap = gen.enum([ gen.const("HOT", int_t, 11, "COLORMAP_HOT"), ], "colormap") -gen.add('do_dynamic_scaling', bool_t, 0, 'Do dynamic scaling about pixel values or not', True) +gen.add('do_dynamic_scaling', bool_t, 0, 'Do dynamic scaling about pixel values or not', False) gen.add('colormap', int_t, 0, "colormap", -1, -1, 11, edit_method=edit_method_colormap); gen.add('min_image_value', double_t, 0, "Minimum image value for scaling depth/float image.", default=0, min=0); gen.add('max_image_value', double_t, 0, "Maximum image value for scaling depth/float image.", default=0, min=0); diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp index ec127a22e..119b2acdf 100644 --- a/image_view/src/nodelets/image_nodelet.cpp +++ b/image_view/src/nodelets/image_nodelet.cpp @@ -198,7 +198,12 @@ void ImageNodelet::reconfigureCb(image_view::ImageViewConfig &config, uint32_t l void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) { // We want to scale floating point images so that they display nicely - bool do_dynamic_scaling = do_dynamic_scaling_ & (msg->encoding.find("F") != std::string::npos); + bool do_dynamic_scaling; + if (msg->encoding.find("F") != std::string::npos) { + do_dynamic_scaling = true; + } else { + do_dynamic_scaling = do_dynamic_scaling_; + } // Convert to OpenCV native BGR color cv_bridge::CvImageConstPtr cv_ptr;