41 #include <opencv2/opencv.hpp> 44 #include <sensor_msgs/CameraInfo.h> 45 #include <dynamic_reconfigure/server.h> 55 #include "opencv_apps/AddingImagesConfig.h" 75 typedef opencv_apps::AddingImagesConfig
Config;
101 const sensor_msgs::CameraInfoConstPtr& cam_info)
103 doWork(msg1, msg2, cam_info->header.frame_id);
106 void imageCallback(
const sensor_msgs::ImageConstPtr& msg1,
const sensor_msgs::ImageConstPtr& msg2)
108 doWork(msg1, msg2, msg1->header.frame_id);
114 sub_image1_.
subscribe(*it_,
"image1", 3);
115 sub_image2_.
subscribe(*it_,
"image2", 3);
117 if (config_.use_camera_info)
119 if (approximate_sync_)
122 boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicyWithCameraInfo> >(
queue_size_);
123 async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_);
130 sync_with_info_ = boost::make_shared<message_filters::Synchronizer<SyncPolicyWithCameraInfo> >(
queue_size_);
131 sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_);
139 if (approximate_sync_)
141 async_ = boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicy> >(
queue_size_);
142 async_->connectInput(sub_image1_, sub_image2_);
143 async_->registerCallback(
148 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
149 sync_->connectInput(sub_image1_, sub_image2_);
150 sync_->registerCallback(
166 boost::mutex::scoped_lock lock(mutex_);
168 alpha_ = config.alpha;
169 if (config.auto_beta)
178 gamma_ = config.gamma;
181 void doWork(
const sensor_msgs::Image::ConstPtr& image_msg1,
const sensor_msgs::Image::ConstPtr& image_msg2,
182 const std::string& input_frame_from_msg)
184 boost::mutex::scoped_lock lock(mutex_);
192 NODELET_ERROR(
"Encoding of input images must be same type: %s, %s", image_msg1->encoding.c_str(),
193 image_msg2->encoding.c_str());
197 cv::Mat result_image;
198 if (image1.rows != image2.rows || image1.cols != image2.cols)
200 int new_rows = std::max(image1.rows, image2.rows);
201 int new_cols = std::max(image1.cols, image2.cols);
203 cv::Mat image1_tmp = cv::Mat(new_rows, new_cols, image1.type());
204 image1.copyTo(image1_tmp(cv::Rect(0, 0, image1.cols, image1.rows)));
205 image1 = image1_tmp.clone();
208 cv::Mat image2_tmp = cv::Mat(new_rows, new_cols, image2.type());
209 image2.copyTo(image2_tmp(cv::Rect(0, 0, image2.cols, image2.rows)));
210 image2 = image2_tmp.clone();
212 cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image);
214 sensor_msgs::ImagePtr image_msg3 =
218 cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
219 #ifdef CV_BRIDGE_CVT_COLOR_FOR_DISPLAY_IS_NOT_SUPPORTED 220 cv::imshow(window_name_, result_image);
222 #ifdef CV_BRIDGE_CVT_COLOR_FOR_DISPLAY_OPTION_IS_NOT_SUPPORTED 223 cv::imshow(window_name_,
236 int c = cv::waitKey(1);
240 catch (cv::Exception& e)
242 NODELET_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
245 prev_stamp_ = image_msg1->header.stamp;
254 pnh_->param(
"debug_view", debug_view_,
false);
261 window_name_ =
"AddingImages Demo";
265 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
266 dynamic_reconfigure::Server<Config>::CallbackType
f =
268 reconfigure_server_->setCallback(f);
270 pnh_->param(
"approximate_sync", approximate_sync_,
true);
271 pnh_->param(
"queue_size", queue_size_, 100);
285 ROS_WARN(
"DeprecationWarning: Nodelet adding_images/adding_images is deprecated, " 286 "and renamed to opencv_apps/adding_images.");
292 #ifdef USE_PLUGINLIB_CLASS_LIST_MACROS_H CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_ERROR(...)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image > SyncPolicy
image_transport::Publisher img_pub_
Demo code to calculate moments.
void reconfigureCallback(Config &config, uint32_t level)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image > ApproxSyncPolicy
boost::shared_ptr< message_filters::Synchronizer< ApproxSyncPolicyWithCameraInfo > > async_with_info_
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
boost::shared_ptr< image_transport::ImageTransport > it_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicyWithCameraInfo
sensor_msgs::ImagePtr toImageMsg() const
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
void imageCallback(const sensor_msgs::ImageConstPtr &msg1, const sensor_msgs::ImageConstPtr &msg2)
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg1, const sensor_msgs::ImageConstPtr &msg2, const sensor_msgs::CameraInfoConstPtr &cam_info)
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
dynamic_reconfigure::Server< Config > ReconfigureServer
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > ApproxSyncPolicyWithCameraInfo
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
void publish(const sensor_msgs::Image &message) const
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
int getCvType(const std::string &encoding)
PLUGINLIB_EXPORT_CLASS(opencv_apps::AddingImagesNodelet, nodelet::Nodelet)
image_transport::SubscriberFilter sub_image1_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr &source, const std::string &encoding=std::string(), const CvtColorForDisplayOptions options=CvtColorForDisplayOptions())
image_transport::SubscriberFilter sub_image2_
opencv_apps::AddingImagesConfig Config
static int bitDepth(const std::string &encoding)
void doWork(const sensor_msgs::Image::ConstPtr &image_msg1, const sensor_msgs::Image::ConstPtr &image_msg2, const std::string &input_frame_from_msg)
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_camera_info_
boost::shared_ptr< message_filters::Synchronizer< ApproxSyncPolicy > > async_
#define NODELET_DEBUG(...)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicyWithCameraInfo > > sync_with_info_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...