overlay_image_display.cpp
Go to the documentation of this file.
1 // -*- mode: c++; -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #include "overlay_image_display.h"
37 
38 #include <OGRE/OgreMaterialManager.h>
39 #include <OGRE/OgreTextureManager.h>
40 #include <OGRE/OgreTexture.h>
41 #include <OGRE/OgreHardwarePixelBuffer.h>
42 #include <OGRE/OgreTechnique.h>
43 
45 #include <cv_bridge/cv_bridge.h>
47 
48 namespace jsk_rviz_plugins
49 {
50 
52  : Display(), width_(128), height_(128), left_(128), top_(128), alpha_(0.8),
53  is_msg_available_(false), require_update_(false), overwrite_alpha_(false)
54  {
55  // setup properties
57  "Topic", "",
58  ros::message_traits::datatype<sensor_msgs::Image>(),
59  "sensor_msgs::Image topic to subscribe to.",
60  this, SLOT( updateTopic() ));
62  "transport hint to subscribe topic",
63  this, SLOT(updateTopic()));
64  keep_aspect_ratio_property_ = new rviz::BoolProperty("keep aspect ratio", false,
65  "keep aspect ratio of original image",
66  this, SLOT(updateKeepAspectRatio()));
67  width_property_ = new rviz::IntProperty("width", 128,
68  "width of the image window",
69  this, SLOT(updateWidth()));
70  height_property_ = new rviz::IntProperty("height", 128,
71  "height of the image window",
72  this, SLOT(updateHeight()));
73  left_property_ = new rviz::IntProperty("left", 128,
74  "left of the image window",
75  this, SLOT(updateLeft()));
76  top_property_ = new rviz::IntProperty("top", 128,
77  "top of the image window",
78  this, SLOT(updateTop()));
79  alpha_property_ = new rviz::FloatProperty("alpha", 0.8,
80  "alpha belnding value",
81  this, SLOT(updateAlpha()));
82  overwrite_alpha_property_ = new rviz::BoolProperty("overwrite alpha value", false,
83  "overwrite alpha value by alpha property "
84  "and ignore alpha channel of the image",
85  this, SLOT(updateOverwriteAlpha()));
86  }
87 
89  {
94  delete width_property_;
95  delete height_property_;
96  delete left_property_;
97  delete top_property_;
98  delete alpha_property_;
99  }
100 
102  {
103  ros::NodeHandle nh;
104 #if ROS_VERSION_MINIMUM(1,12,0)
105  it_ = std::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh));
106 #else
108 #endif
109 
110  updateWidth();
111  updateHeight();
114  updateTop();
115  updateLeft();
116  updateAlpha();
117  updateTopic();
118  }
119 
121  {
122  if (overlay_) {
123  overlay_->show();
124  }
125  subscribe();
126  }
128  {
129  if (overlay_) {
130  overlay_->hide();
131  }
132  unsubscribe();
133  }
134 
136  {
137  sub_.shutdown();
138  // clear clear clear...
139  }
140 
142  {
143  if (isEnabled()) {
144  std::string topic_name = update_topic_property_->getTopicStd();
145 
146  if (topic_name.length() > 0 && topic_name != "/") {
147  const image_transport::TransportHints transport_hint =
149  sub_ = it_->subscribe(topic_name, 1, &OverlayImageDisplay::processMessage, this,
150  transport_hint);
151  }
152  }
153  }
154 
156  const sensor_msgs::Image::ConstPtr& msg)
157  {
158  msg_ = msg;
159  is_msg_available_ = true;
160  require_update_ = true;
161  if ((width_property_->getInt() < 0) || (height_property_->getInt() < 0) || keep_aspect_ratio_) {
162  // automatically setup display size
163  updateWidth();
164  updateHeight();
165  }
166  }
167 
168 
169  void OverlayImageDisplay::update(float wall_dt, float ros_dt)
170  {
171  if (!isEnabled()) {
172  return;
173  }
174 
176  if (!overlay_) {
177  static int count = 0;
179  ss << "OverlayImageDisplayObject" << count++;
180  overlay_.reset(new OverlayObject(ss.str()));
181  overlay_->show();
182  }
183  overlay_->updateTextureSize(msg_->width, msg_->height);
184  // When aspect_ratio being kept, the size is specified by width;
186  setImageSize();
187  redraw();
188  require_update_ = false;
189  }
190  if (overlay_) {
191  overlay_->setDimensions(width_, height_);
192  overlay_->setPosition(left_, top_);
193  }
194  }
195 
197  {
198  try
199  {
200  if (msg_->width == 0 || msg_->height == 0) {
201  // image width/height and texture width/height should be same
202  // but they are not when input image width/height is 0
203  return;
204  }
205  else {
206  cv::Mat mat; // mat should be BGRA8 image
207 
208  if ((msg_->encoding == sensor_msgs::image_encodings::BGRA8 ||
210  !overwrite_alpha_) {
213  cv_ptr->image.copyTo(mat);
214  } else {
215  // If the image does not have alpha channel, use alpha_ value.
216  const cv_bridge::CvImagePtr cv_ptr =
218  const cv::Mat bgr_image = cv_ptr->image;
219  std::vector<cv::Mat> channels;
220  // Split BGR image to each channel because cv::merge requires 4 images to create
221  // B-G-R-A image. The each 4 image represents each channel.
222  cv::split(bgr_image, channels);
223  // Create single alpha channel image
224  const cv::Mat alpha(bgr_image.rows, bgr_image.cols, CV_8UC1,
225  cv::Scalar(alpha_ * 255.0));
226  channels.push_back(alpha);
227  cv::merge(channels, mat);
228  }
229 
230  ScopedPixelBuffer buffer = overlay_->getBuffer();
231  QImage Hud = buffer.getQImage(*overlay_);
232  // QImage created from ScopedPixelBuffer has no padding between each line.
233  memcpy(Hud.scanLine(0), mat.data, mat.cols * mat.rows * mat.elemSize());
234  }
235  }
236  catch (cv_bridge::Exception& e)
237  {
238  ROS_ERROR("cv_bridge exception: %s", e.what());
239  }
240  }
241 
243  {
244  unsubscribe();
245  subscribe();
246  }
247 
249  {
250  if (width_ == -1) {
251  if (is_msg_available_) {
252  width_ = msg_->width;
253  }
254  else {
255  width_ = 128;
256  }
257  }
258 
259  if (height_ == -1) {
260  if (is_msg_available_) {
261  height_ = msg_->height;
262  }
263  else {
264  height_ = 128;
265  }
266  }
267 
269  // When aspect_ratio being kept, the size is specified by width;
270  double aspect_ratio = msg_->height / (double)msg_->width;
271  int height_from_width = std::ceil(width_ * aspect_ratio);
272  height_ = height_from_width;
273  }
274 
275  }
276 
278  {
279  boost::mutex::scoped_lock lock(mutex_);
281  require_update_ = true;
282  }
283 
285  {
286  boost::mutex::scoped_lock lock(mutex_);
288  require_update_ = true;
289  }
290 
292  {
293  boost::mutex::scoped_lock lock(mutex_);
295  }
296 
298  {
299  boost::mutex::scoped_lock lock(mutex_);
301  }
302 
304  {
305  boost::mutex::scoped_lock lock(mutex_);
307  }
308 
310  {
311  boost::mutex::scoped_lock lock(mutex_);
313  require_update_ = true;
314  }
315 
317  {
318  boost::mutex::scoped_lock lock(mutex_);
320  require_update_ = true;
321  }
322 
324  {
325  return (top_ < y && top_ + height_ > y &&
326  left_ < x && left_ + width_ > x);
327  }
328 
330  {
331  top_ = y;
332  left_ = x;
333  }
334 
336  {
339  }
340 
341 }
342 
virtual bool setValue(const QVariant &new_value)
virtual QImage getQImage(unsigned int width, unsigned int height)
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
virtual int getInt() const
virtual float getFloat() const
bool isEnabled() const
virtual bool getBool() const
virtual void processMessage(const sensor_msgs::Image::ConstPtr &msg)
ImageTransportHintsProperty * transport_hint_property_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void setHidden(bool hidden)
boost::shared_ptr< image_transport::ImageTransport > it_
std::string getTopicStd() const
virtual void update(float wall_dt, float ros_dt)
#define ROS_ERROR(...)
rviz::RosTopicProperty * update_topic_property_


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18