simple_compressed_example_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Tal Regev.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Kei Okada nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 // http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
40 #include <ros/ros.h>
41 #include <sensor_msgs/CompressedImage.h>
43 #include <nodelet/nodelet.h>
45 #include <cv_bridge/cv_bridge.h>
47 
48 #include <opencv2/highgui/highgui.hpp>
49 #include <opencv2/imgproc/imgproc.hpp>
50 
51 #include <dynamic_reconfigure/server.h>
52 
53 namespace opencv_apps
54 {
56 {
57 static const std::string OPENCV_WINDOW = "Image window";
58 
60 {
66 
67 public:
69  {
70  // Subscrive to input video feed and publish output video feed
71  image_sub_ = nh_.subscribe("image/compressed", queue_size_, &ImageConverter::imageCb, this);
72  image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>("/image_converter/output_video/compressed", 1);
73 
74  ros::NodeHandle pnh("~");
75  pnh.param("queue_size", queue_size_, 3);
76  pnh.param("debug_view", debug_view_, false);
77  if (debug_view_)
78  {
79  cv::namedWindow(OPENCV_WINDOW);
80  }
81  }
82 
84  {
85  if (debug_view_)
86  {
87  cv::destroyWindow(OPENCV_WINDOW);
88  }
89  }
90 
91  void imageCb(const sensor_msgs::CompressedImageConstPtr& msg)
92  {
93  cv_bridge::CvImagePtr cv_ptr;
94  try
95  {
96 /*
97 Supporting CompressedImage in cv_bridge has been started from 1.11.9 (2015-11-29)
98 note : hydro 1.10.18, indigo : 1.11.13 ...
99 https://github.com/ros-perception/vision_opencv/pull/70
100  */
101 #ifndef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED
103 #else
104  // cv_ptr = cv_bridge::toCvCopyImpl(matFromImage(msg), msg.header, sensor_msgs::image_encodings::BGR8,
105  // sensor_msgs::image_encodings::BGR8);
106  // cv::Mat matFromImage(const sensor_msgs::CompressedImage& source)
107  cv::Mat jpegData(1, msg->data.size(), CV_8UC1);
108  jpegData.data = const_cast<uchar*>(&msg->data[0]);
109  cv::InputArray data(jpegData);
110  cv::Mat image = cv::imdecode(data, cv::IMREAD_COLOR);
111 
112  // cv_ptr = cv_bridge::toCvCopyImpl(bgrMat, msg->header, sensor_msgs::image_encodings::BGR8,
113  // sensor_msgs::image_encodings::BGR8);
114  sensor_msgs::Image ros_image;
115  ros_image.header = msg->header;
116  ros_image.height = image.rows;
117  ros_image.width = image.cols;
118  ros_image.encoding = sensor_msgs::image_encodings::BGR8;
119  ros_image.is_bigendian = false;
120  ros_image.step = image.cols * image.elemSize();
121  size_t size = ros_image.step * image.rows;
122  ros_image.data.resize(size);
123 
124  if (image.isContinuous())
125  {
126  memcpy((char*)(&ros_image.data[0]), image.data, size);
127  }
128  else
129  {
130  // Copy by row by row
131  uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]);
132  uchar* cv_data_ptr = image.data;
133  for (int i = 0; i < image.rows; ++i)
134  {
135  memcpy(ros_data_ptr, cv_data_ptr, ros_image.step);
136  ros_data_ptr += ros_image.step;
137  cv_data_ptr += image.step;
138  }
139  }
141 #endif
142  }
143  catch (cv_bridge::Exception& e)
144  {
145  ROS_ERROR("cv_bridge exception: %s", e.what());
146  return;
147  }
148 
149  // Draw an example circle on the video stream
150  if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110)
151  cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols / 2, cv_ptr->image.rows / 2), 100, CV_RGB(255, 0, 0));
152 
153  if (debug_view_)
154  {
155  // Update GUI Window
156  cv::imshow(OPENCV_WINDOW, cv_ptr->image);
157  cv::waitKey(3);
158  }
159 
160 // Output modified video stream
161 #ifndef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED
162  image_pub_.publish(cv_ptr->toCompressedImageMsg());
163 #else
164  image_pub_.publish(toCompressedImageMsg(cv_ptr));
165 #endif
166  }
167 #ifdef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED
168  sensor_msgs::CompressedImage toCompressedImageMsg(cv_bridge::CvImagePtr cv_ptr) const
169  {
170  sensor_msgs::CompressedImage ros_image;
171  const std::string dst_format = std::string();
172  ros_image.header = cv_ptr->header;
173  cv::Mat image;
174  if (cv_ptr->encoding != sensor_msgs::image_encodings::BGR8)
175  {
176  cv_bridge::CvImagePtr tempThis = boost::make_shared<cv_bridge::CvImage>(*cv_ptr);
178  cv_ptr->image = temp->image;
179  }
180  else
181  {
182  image = cv_ptr->image;
183  }
184  std::vector<uchar> buf;
185  if (dst_format.empty() || dst_format == "jpg")
186  {
187  ros_image.format = "jpg";
188  cv::imencode(".jpg", image, buf);
189  }
190 
191  if (dst_format == "png")
192  {
193  ros_image.format = "png";
194  cv::imencode(".png", image, buf);
195  }
196 
197  // TODO: check this formats (on rviz) and add more formats
198  // from http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const
199  // string& filename, int flags)
200  if (dst_format == "jp2")
201  {
202  ros_image.format = "jp2";
203  cv::imencode(".jp2", image, buf);
204  }
205 
206  if (dst_format == "bmp")
207  {
208  ros_image.format = "bmp";
209  cv::imencode(".bmp", image, buf);
210  }
211 
212  if (dst_format == "tif")
213  {
214  ros_image.format = "tif";
215  cv::imencode(".tif", image, buf);
216  }
217 
218  ros_image.data = buf;
219  return ros_image;
220  }
221 #endif
222 };
223 
224 } // namespace simple_compressed_example
225 
227 {
228 public:
229  virtual void onInit() // NOLINT(modernize-use-override)
230  {
232  ros::spin();
233  }
234 };
235 
236 } // namespace opencv_apps
237 
239 {
241 {
242 public:
243  virtual void onInit() // NOLINT(modernize-use-override)
244  {
245  ROS_WARN("DeprecationWarning: Nodelet simple_compressed_example/simple_compressed_example is deprecated, "
246  "and renamed to opencv_apps/simple_compressed_example.");
248  }
249 };
250 } // namespace simple_compressed_example
251 
void publish(const boost::shared_ptr< M > &message) const
void imageCb(const sensor_msgs::CompressedImageConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
Demo code to calculate moments.
Definition: nodelet.h:48
#define ROS_WARN(...)
ROSCPP_DECL void spin(Spinner &spinner)
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
PLUGINLIB_EXPORT_CLASS(opencv_apps::SimpleCompressedExampleNodelet, nodelet::Nodelet)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_ERROR(...)


opencv_apps
Author(s): Kei Okada
autogenerated on Sat Aug 22 2020 03:35:08