From 530281a12fc17de483e8dfb06a04eeb6ab397c72 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Wed, 13 Feb 2019 13:43:33 +0100 Subject: [PATCH 1/7] Removed hard coded image encoding. Using toCvCopy instead of toCvShared (copy is needed anyway). --- image_proc/src/nodelets/resize.cpp | 149 ++++++++++++++++------------- 1 file changed, 83 insertions(+), 66 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index 44fc9a30e..b8856c458 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -31,27 +31,35 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include +#include +#if ((BOOST_VERSION / 100) % 1000) >= 53 +#include +#endif + #include #include #include -#include #include #include #include +#include #include "image_proc/ResizeConfig.h" namespace image_proc { -class ResizeNodelet : public nodelet_topic_tools::NodeletLazy +class ResizeNodelet : public nodelet::Nodelet { protected: // ROS communication - ros::Publisher pub_image_; - ros::Publisher pub_info_; - ros::Subscriber sub_info_; - ros::Subscriber sub_image_; + image_transport::CameraPublisher pub_image_; + image_transport::CameraSubscriber sub_image_; + int queue_size_; + + boost::shared_ptr it_, private_it_; + boost::mutex connect_mutex_; // Dynamic reconfigure boost::recursive_mutex config_mutex_; @@ -61,10 +69,10 @@ class ResizeNodelet : public nodelet_topic_tools::NodeletLazy Config config_; virtual void onInit(); - virtual void subscribe(); - virtual void unsubscribe(); - void imageCb(const sensor_msgs::ImageConstPtr& image_msg); + void connectCb(); + + void imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg); void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); void configCb(Config &config, uint32_t level); @@ -72,87 +80,65 @@ class ResizeNodelet : public nodelet_topic_tools::NodeletLazy void ResizeNodelet::onInit() { - nodelet_topic_tools::NodeletLazy::onInit(); + ros::NodeHandle &nh = getNodeHandle(); + ros::NodeHandle &private_nh = getPrivateNodeHandle(); + it_.reset(new image_transport::ImageTransport(nh)); + private_it_.reset(new image_transport::ImageTransport(private_nh)); // Set up dynamic reconfigure - reconfigure_server_.reset(new ReconfigureServer(config_mutex_, *pnh_)); + reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh)); ReconfigureServer::CallbackType f = boost::bind(&ResizeNodelet::configCb, this, _1, _2); reconfigure_server_->setCallback(f); - pub_info_ = advertise(*pnh_, "camera_info", 1); - pub_image_ = advertise(*pnh_, "image", 1); + // Monitor whether anyone is subscribed to the output + typedef image_transport::SubscriberStatusCallback ConnectCB; + ConnectCB connect_cb = boost::bind(&ResizeNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX + boost::lock_guard lock(connect_mutex_); - onInitPostProcess(); + pub_image_ = private_it_->advertiseCamera("image", 1, connect_cb, connect_cb); } -void ResizeNodelet::configCb(Config &config, uint32_t level) +// Handles (un)subscribing when clients (un)subscribe +void ResizeNodelet::connectCb() { - config_ = config; -} - -void ResizeNodelet::subscribe() -{ - sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this); - sub_image_ = nh_->subscribe("image", 1, &ResizeNodelet::imageCb, this); + boost::lock_guard lock(connect_mutex_); + if (pub_image_.getNumSubscribers() == 0) + { + sub_image_.shutdown(); + } + else if (!sub_image_) + { + image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); + sub_image_ = it_->subscribeCamera("image", queue_size_, &ResizeNodelet::imageCb, this, hints); + } } -void ResizeNodelet::unsubscribe() +void ResizeNodelet::configCb(Config &config, uint32_t level) { - sub_info_.shutdown(); - sub_image_.shutdown(); + config_ = config; } -void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) +void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg) { Config config; { boost::lock_guard lock(config_mutex_); config = config_; } - - sensor_msgs::CameraInfo dst_info_msg = *info_msg; - - double scale_y; - double scale_x; - if (config.use_scale) + // image + cv_bridge::CvImagePtr cv_ptr; + try { - scale_y = config.scale_height; - scale_x = config.scale_width; - dst_info_msg.height = static_cast(info_msg->height * config.scale_height); - dst_info_msg.width = static_cast(info_msg->width * config.scale_width); - } - else - { - scale_y = static_cast(config.height) / info_msg->height; - scale_x = static_cast(config.width) / info_msg->width; - dst_info_msg.height = config.height; - dst_info_msg.width = config.width; + cv_ptr = cv_bridge::toCvCopy(image_msg); } - - dst_info_msg.K[0] = dst_info_msg.K[0] * scale_x; // fx - dst_info_msg.K[2] = dst_info_msg.K[2] * scale_x; // cx - dst_info_msg.K[4] = dst_info_msg.K[4] * scale_y; // fy - dst_info_msg.K[5] = dst_info_msg.K[5] * scale_y; // cy - - dst_info_msg.P[0] = dst_info_msg.P[0] * scale_x; // fx - dst_info_msg.P[2] = dst_info_msg.P[2] * scale_x; // cx - dst_info_msg.P[3] = dst_info_msg.P[3] * scale_x; // T - dst_info_msg.P[5] = dst_info_msg.P[5] * scale_y; // fy - dst_info_msg.P[6] = dst_info_msg.P[6] * scale_y; // cy - - pub_info_.publish(dst_info_msg); -} - -void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg) -{ - Config config; + catch (cv_bridge::Exception& e) { - boost::lock_guard lock(config_mutex_); - config = config_; + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; } - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg); - if (config.use_scale) { cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), @@ -165,7 +151,38 @@ void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg) cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, config.interpolation); } - pub_image_.publish(cv_ptr->toImageMsg()); + // camera_info + sensor_msgs::CameraInfoPtr dst_info_msg(new sensor_msgs::CameraInfo(*info_msg)); + + double scale_y; + double scale_x; + if (config.use_scale) + { + scale_y = config.scale_height; + scale_x = config.scale_width; + dst_info_msg->height = static_cast(info_msg->height * config.scale_height); + dst_info_msg->width = static_cast(info_msg->width * config.scale_width); + } + else + { + scale_y = static_cast(config.height) / info_msg->height; + scale_x = static_cast(config.width) / info_msg->width; + dst_info_msg->height = config.height; + dst_info_msg->width = config.width; + } + + dst_info_msg->K[0] = dst_info_msg->K[0] * scale_x; // fx + dst_info_msg->K[2] = dst_info_msg->K[2] * scale_x; // cx + dst_info_msg->K[4] = dst_info_msg->K[4] * scale_y; // fy + dst_info_msg->K[5] = dst_info_msg->K[5] * scale_y; // cy + + dst_info_msg->P[0] = dst_info_msg->P[0] * scale_x; // fx + dst_info_msg->P[2] = dst_info_msg->P[2] * scale_x; // cx + dst_info_msg->P[3] = dst_info_msg->P[3] * scale_x; // T + dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y; // fy + dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y; // cy + + pub_image_.publish(cv_ptr->toImageMsg(), dst_info_msg); } } // namespace image_proc From ba4559751df731cd179a62ed74c39181c0ea4064 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Wed, 13 Feb 2019 13:43:33 +0100 Subject: [PATCH 2/7] replaced boost mutex & shared_ptr with std --- image_proc/src/nodelets/resize.cpp | 109 ++++++++++++++--------------- 1 file changed, 52 insertions(+), 57 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index b8856c458..71429bb5e 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -1,42 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2017, Kentaro Wada. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Kentaro Wada nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ -#include -#include -#if ((BOOST_VERSION / 100) % 1000) >= 53 -#include -#endif - + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Kentaro Wada. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Kentaro Wada nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#include #include #include #include @@ -49,7 +44,6 @@ namespace image_proc { - class ResizeNodelet : public nodelet::Nodelet { protected: @@ -58,51 +52,51 @@ class ResizeNodelet : public nodelet::Nodelet image_transport::CameraSubscriber sub_image_; int queue_size_; - boost::shared_ptr it_, private_it_; - boost::mutex connect_mutex_; + std::shared_ptr it_, private_it_; + std::mutex connect_mutex_; // Dynamic reconfigure - boost::recursive_mutex config_mutex_; + std::mutex config_mutex_; typedef image_proc::ResizeConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; + std::shared_ptr reconfigure_server_; Config config_; virtual void onInit(); void connectCb(); - void imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg); + void imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); - void configCb(Config &config, uint32_t level); + void configCb(Config& config, uint32_t level); }; void ResizeNodelet::onInit() { - ros::NodeHandle &nh = getNodeHandle(); - ros::NodeHandle &private_nh = getPrivateNodeHandle(); + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& private_nh = getPrivateNodeHandle(); it_.reset(new image_transport::ImageTransport(nh)); private_it_.reset(new image_transport::ImageTransport(private_nh)); // Set up dynamic reconfigure - reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh)); - ReconfigureServer::CallbackType f = boost::bind(&ResizeNodelet::configCb, this, _1, _2); + reconfigure_server_.reset(new ReconfigureServer(private_nh)); + ReconfigureServer::CallbackType f = + std::bind(&ResizeNodelet::configCb, this, std::placeholders::_1, std::placeholders::_2); reconfigure_server_->setCallback(f); // Monitor whether anyone is subscribed to the output typedef image_transport::SubscriberStatusCallback ConnectCB; - ConnectCB connect_cb = boost::bind(&ResizeNodelet::connectCb, this); + ConnectCB connect_cb = std::bind(&ResizeNodelet::connectCb, this); // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX - boost::lock_guard lock(connect_mutex_); - + std::lock_guard lock(connect_mutex_); pub_image_ = private_it_->advertiseCamera("image", 1, connect_cb, connect_cb); } // Handles (un)subscribing when clients (un)subscribe void ResizeNodelet::connectCb() { - boost::lock_guard lock(connect_mutex_); + std::lock_guard lock(connect_mutex_); if (pub_image_.getNumSubscribers() == 0) { sub_image_.shutdown(); @@ -114,8 +108,9 @@ void ResizeNodelet::connectCb() } } -void ResizeNodelet::configCb(Config &config, uint32_t level) +void ResizeNodelet::configCb(Config& config, uint32_t level) { + std::lock_guard lock(config_mutex_); config_ = config; } @@ -124,7 +119,7 @@ void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, { Config config; { - boost::lock_guard lock(config_mutex_); + std::lock_guard lock(config_mutex_); config = config_; } // image @@ -141,8 +136,8 @@ void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, if (config.use_scale) { - cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), - config.scale_width, config.scale_height, config.interpolation); + cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), config.scale_width, config.scale_height, + config.interpolation); } else { From 5f2617be048769c125af165bb84f1f05af913749 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Thu, 2 May 2019 17:14:45 +0200 Subject: [PATCH 3/7] Rename infoCb to cameraCb matching subscribeCamera --- image_proc/src/nodelets/resize.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index 71429bb5e..329c0d4c6 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -66,7 +66,7 @@ class ResizeNodelet : public nodelet::Nodelet void connectCb(); - void imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); + void cameraCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); void configCb(Config& config, uint32_t level); @@ -104,7 +104,7 @@ void ResizeNodelet::connectCb() else if (!sub_image_) { image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_image_ = it_->subscribeCamera("image", queue_size_, &ResizeNodelet::imageCb, this, hints); + sub_image_ = it_->subscribeCamera("image", queue_size_, &ResizeNodelet::cameraCb, this, hints); } } @@ -114,7 +114,7 @@ void ResizeNodelet::configCb(Config& config, uint32_t level) config_ = config; } -void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, +void ResizeNodelet::cameraCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { Config config; From 53f6ef8049b062baea3187bd12b35aac26394e08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Thu, 2 May 2019 19:20:01 +0200 Subject: [PATCH 4/7] removed unused infoCb --- image_proc/src/nodelets/resize.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index 329c0d4c6..a582a18b7 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -65,10 +65,7 @@ class ResizeNodelet : public nodelet::Nodelet virtual void onInit(); void connectCb(); - void cameraCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); - void configCb(Config& config, uint32_t level); }; From e8465eaa402dcd8bdb23bf1d85fe92bf0c1e49b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Wed, 15 May 2019 13:11:23 +0200 Subject: [PATCH 5/7] Removed indendation of license --- image_proc/src/nodelets/resize.cpp | 64 +++++++++++++++--------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index a582a18b7..341892551 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -1,36 +1,36 @@ /********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Kentaro Wada. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Kentaro Wada nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ +* Software License Agreement (BSD License) +* +* Copyright (c) 2017, Kentaro Wada. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Kentaro Wada nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ #include #include #include From 168dbee3bda89bb15c49ae50a7536b42a1ae3762 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Wed, 15 May 2019 16:26:14 +0200 Subject: [PATCH 6/7] Independent resize of image and camera_info --- image_proc/src/nodelets/resize.cpp | 108 +++++++++++++++++------------ 1 file changed, 65 insertions(+), 43 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index 341892551..ee36d2965 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -48,9 +48,12 @@ class ResizeNodelet : public nodelet::Nodelet { protected: // ROS communication - image_transport::CameraPublisher pub_image_; - image_transport::CameraSubscriber sub_image_; - int queue_size_; + std::shared_ptr nh_; + std::shared_ptr pnh_; + image_transport::Publisher pub_image_; + ros::Publisher pub_info_; + image_transport::Subscriber sub_image_; + ros::Subscriber sub_info_; std::shared_ptr it_, private_it_; std::mutex connect_mutex_; @@ -63,31 +66,34 @@ class ResizeNodelet : public nodelet::Nodelet Config config_; virtual void onInit(); - void connectCb(); - void cameraCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); + + void imageCb(const sensor_msgs::ImageConstPtr& image_msg); + void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); + void configCb(Config& config, uint32_t level); }; void ResizeNodelet::onInit() { - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - it_.reset(new image_transport::ImageTransport(nh)); - private_it_.reset(new image_transport::ImageTransport(private_nh)); + nh_.reset(new ros::NodeHandle(getNodeHandle())); + pnh_.reset(new ros::NodeHandle(getPrivateNodeHandle())); + it_.reset(new image_transport::ImageTransport(*nh_)); + private_it_.reset(new image_transport::ImageTransport(*pnh_)); // Set up dynamic reconfigure - reconfigure_server_.reset(new ReconfigureServer(private_nh)); + reconfigure_server_.reset(new ReconfigureServer(*pnh_)); ReconfigureServer::CallbackType f = std::bind(&ResizeNodelet::configCb, this, std::placeholders::_1, std::placeholders::_2); reconfigure_server_->setCallback(f); // Monitor whether anyone is subscribed to the output - typedef image_transport::SubscriberStatusCallback ConnectCB; - ConnectCB connect_cb = std::bind(&ResizeNodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX + auto connect_cb = std::bind(&ResizeNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to + // pub_XXX std::lock_guard lock(connect_mutex_); - pub_image_ = private_it_->advertiseCamera("image", 1, connect_cb, connect_cb); + pub_image_ = private_it_->advertise("image", 1, connect_cb, connect_cb); + pub_info_ = pnh_->advertise("camera_info", 1, connect_cb, connect_cb); } // Handles (un)subscribing when clients (un)subscribe @@ -100,8 +106,15 @@ void ResizeNodelet::connectCb() } else if (!sub_image_) { - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_image_ = it_->subscribeCamera("image", queue_size_, &ResizeNodelet::cameraCb, this, hints); + sub_image_ = it_->subscribe("image", 1, &ResizeNodelet::imageCb, this); + } + if (pub_info_.getNumSubscribers() == 0) + { + sub_info_.shutdown(); + } + else if (!sub_info_) + { + sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this); } } @@ -111,39 +124,14 @@ void ResizeNodelet::configCb(Config& config, uint32_t level) config_ = config; } -void ResizeNodelet::cameraCb(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg) +void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) { Config config; { std::lock_guard lock(config_mutex_); config = config_; } - // image - cv_bridge::CvImagePtr cv_ptr; - try - { - cv_ptr = cv_bridge::toCvCopy(image_msg); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - if (config.use_scale) - { - cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), config.scale_width, config.scale_height, - config.interpolation); - } - else - { - int height = config.height == -1 ? image_msg->height : config.height; - int width = config.width == -1 ? image_msg->width : config.width; - cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, config.interpolation); - } - - // camera_info sensor_msgs::CameraInfoPtr dst_info_msg(new sensor_msgs::CameraInfo(*info_msg)); double scale_y; @@ -174,7 +162,41 @@ void ResizeNodelet::cameraCb(const sensor_msgs::ImageConstPtr& image_msg, dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y; // fy dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y; // cy - pub_image_.publish(cv_ptr->toImageMsg(), dst_info_msg); + pub_info_.publish(dst_info_msg); +} + +void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg) +{ + Config config; + { + std::lock_guard lock(config_mutex_); + config = config_; + } + + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(image_msg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + if (config.use_scale) + { + cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), config.scale_width, config.scale_height, + config.interpolation); + } + else + { + int height = config.height == -1 ? image_msg->height : config.height; + int width = config.width == -1 ? image_msg->width : config.width; + cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, config.interpolation); + } + + pub_image_.publish(cv_ptr->toImageMsg()); } } // namespace image_proc From dda7acb88d3c0fbbf8d253619a5ff7d7089e293d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tim=20=C3=9Cbelh=C3=B6r?= Date: Wed, 15 May 2019 16:33:03 +0200 Subject: [PATCH 7/7] Simplified copying of the camera_info message. --- image_proc/src/nodelets/resize.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index ee36d2965..cf2ef20cb 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -132,7 +132,7 @@ void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) config = config_; } - sensor_msgs::CameraInfoPtr dst_info_msg(new sensor_msgs::CameraInfo(*info_msg)); + sensor_msgs::CameraInfo dst_info_msg = *info_msg; double scale_y; double scale_x; @@ -140,27 +140,27 @@ void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) { scale_y = config.scale_height; scale_x = config.scale_width; - dst_info_msg->height = static_cast(info_msg->height * config.scale_height); - dst_info_msg->width = static_cast(info_msg->width * config.scale_width); + dst_info_msg.height = static_cast(info_msg->height * config.scale_height); + dst_info_msg.width = static_cast(info_msg->width * config.scale_width); } else { scale_y = static_cast(config.height) / info_msg->height; scale_x = static_cast(config.width) / info_msg->width; - dst_info_msg->height = config.height; - dst_info_msg->width = config.width; + dst_info_msg.height = config.height; + dst_info_msg.width = config.width; } - dst_info_msg->K[0] = dst_info_msg->K[0] * scale_x; // fx - dst_info_msg->K[2] = dst_info_msg->K[2] * scale_x; // cx - dst_info_msg->K[4] = dst_info_msg->K[4] * scale_y; // fy - dst_info_msg->K[5] = dst_info_msg->K[5] * scale_y; // cy + dst_info_msg.K[0] = dst_info_msg.K[0] * scale_x; // fx + dst_info_msg.K[2] = dst_info_msg.K[2] * scale_x; // cx + dst_info_msg.K[4] = dst_info_msg.K[4] * scale_y; // fy + dst_info_msg.K[5] = dst_info_msg.K[5] * scale_y; // cy - dst_info_msg->P[0] = dst_info_msg->P[0] * scale_x; // fx - dst_info_msg->P[2] = dst_info_msg->P[2] * scale_x; // cx - dst_info_msg->P[3] = dst_info_msg->P[3] * scale_x; // T - dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y; // fy - dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y; // cy + dst_info_msg.P[0] = dst_info_msg.P[0] * scale_x; // fx + dst_info_msg.P[2] = dst_info_msg.P[2] * scale_x; // cx + dst_info_msg.P[3] = dst_info_msg.P[3] * scale_x; // T + dst_info_msg.P[5] = dst_info_msg.P[5] * scale_y; // fy + dst_info_msg.P[6] = dst_info_msg.P[6] * scale_y; // cy pub_info_.publish(dst_info_msg); }