robot_image_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
31 
32 // C++ standard libraries
33 #include <cstdio>
34 #include <algorithm>
35 #include <vector>
36 
37 // QT libraries
38 #include <QGLWidget>
39 #include <QPalette>
40 #include <QImage>
41 #include <QFileDialog>
42 
43 // ROS libraries
44 #include <ros/master.h>
45 #include <ros/package.h>
46 
48 
49 // Declare plugin
52 
53 namespace mapviz_plugins
54 {
56  config_widget_(new QWidget()),
57  width_(2.0),
58  height_(1.0),
59  offset_x_(0.0),
60  offset_y_(0.0),
61  image_ratio_(1.0),
62  texture_loaded_(false),
63  transformed_(false)
64  {
65  ui_.setupUi(config_widget_);
66 
67  // Set background white
68  QPalette p(config_widget_->palette());
69  p.setColor(QPalette::Background, Qt::white);
70  config_widget_->setPalette(p);
71 
72  // Set status text red
73  QPalette p3(ui_.status->palette());
74  p3.setColor(QPalette::Text, Qt::red);
75  ui_.status->setPalette(p3);
76 
77  UpdateShape();
78 
79  QObject::connect(ui_.browse, SIGNAL(clicked()), this, SLOT(SelectFile()));
80  QObject::connect(ui_.selectframe, SIGNAL(clicked()), this, SLOT(SelectFrame()));
81  QObject::connect(ui_.frame, SIGNAL(editingFinished()), this, SLOT(FrameEdited()));
82  QObject::connect(ui_.image, SIGNAL(editingFinished()), this, SLOT(ImageEdited()));
83  QObject::connect(ui_.width, SIGNAL(valueChanged(double)), this, SLOT(WidthChanged(double)));
84  QObject::connect(ui_.height, SIGNAL(valueChanged(double)), this, SLOT(HeightChanged(double)));
85  QObject::connect(ui_.offset_x, SIGNAL(valueChanged(double)), this, SLOT(OffsetXChanged(double)));
86  QObject::connect(ui_.offset_y, SIGNAL(valueChanged(double)), this, SLOT(OffsetYChanged(double)));
87  ui_.offset_x->setMinimum(-99.99); //default is 0.0 but negative offset must be supported
88  ui_.offset_y->setMinimum(-99.99);
89  QObject::connect(ui_.ratio_equal, SIGNAL(toggled(bool)), this, SLOT(RatioEqualToggled(bool)));
90  QObject::connect(ui_.ratio_custom, SIGNAL(toggled(bool)), this, SLOT(RatioCustomToggled(bool)));
91  QObject::connect(ui_.ratio_original, SIGNAL(toggled(bool)), this, SLOT(RatioOriginalToggled(bool)));
92  }
93 
95  {
96  }
97 
99  {
100  QFileDialog dialog(config_widget_, "Select PNG Image");
101  dialog.setFileMode(QFileDialog::ExistingFile);
102  dialog.setNameFilter(tr("PNG Image Files (*.png)"));
103 
104  dialog.exec();
105 
106  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
107  {
108  ui_.image->setText(dialog.selectedFiles().first());
109  filename_ = dialog.selectedFiles().first().toStdString();
110  LoadImage();
111  }
112  }
113 
115  {
116  std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_);
117  if (!frame.empty())
118  {
119  ui_.frame->setText(QString::fromStdString(frame));
120  FrameEdited();
121  }
122  }
123 
125  {
126  filename_ = ui_.image->text().toStdString();
127  LoadImage();
128  }
129 
131  {
132  source_frame_ = ui_.frame->text().toStdString();
133  PrintWarning("Waiting for transform.");
134 
135  ROS_INFO("Setting target frame to to %s", source_frame_.c_str());
136 
137  initialized_ = true;
138 
139  UpdateShape();
140  }
141 
143  {
144  width_ = value;
145  if( ui_.ratio_equal->isChecked()){
146  ui_.height->setValue( width_ );
147  }
148  else if( ui_.ratio_original->isChecked()){
149  ui_.height->setValue( width_ * image_ratio_ );
150  }
151  UpdateShape();
152  }
153 
155  {
156  height_ = value;
157  UpdateShape();
158  }
159 
161  {
162  offset_x_ = value;
163  UpdateShape();
164  }
165 
167  {
168  offset_y_ = value;
169  UpdateShape();
170  }
171 
173  {
174  if( toggled )
175  {
176  ui_.height->setValue(width_);
177  ui_.height->setEnabled(false);
178  UpdateShape();
179  }
180  }
181 
183  {
184  if( toggled )
185  {
186  ui_.height->setEnabled(true);
187  UpdateShape();
188  }
189  }
190 
192  {
193  if( toggled )
194  {
195  ui_.height->setValue(width_*image_ratio_);
196  ui_.height->setEnabled(false);
197  UpdateShape();
198  }
199  }
200 
202  {
203  double hw = 0.5*width_; //half width
204  double hh = 0.5*height_; //half height
205  top_left_ = tf::Point(offset_x_ - hw, offset_y_ + hh, 0);
206  top_right_ = tf::Point(offset_x_ + hw, offset_y_ + hh, 0);
207  bottom_left_ = tf::Point(offset_x_ - hw, offset_y_ - hh, 0);
208  bottom_right_ = tf::Point(offset_x_ + hw, offset_y_ - hh, 0);
209  }
210 
211  void RobotImagePlugin::PrintError(const std::string& message)
212  {
213  PrintErrorHelper(ui_.status, message);
214  }
215 
216  void RobotImagePlugin::PrintInfo(const std::string& message)
217  {
218  PrintInfoHelper(ui_.status, message);
219  }
220 
221  void RobotImagePlugin::PrintWarning(const std::string& message)
222  {
223  PrintWarningHelper(ui_.status, message);
224  }
225 
226  QWidget* RobotImagePlugin::GetConfigWidget(QWidget* parent)
227  {
228  config_widget_->setParent(parent);
229 
230  return config_widget_;
231  }
232 
233  bool RobotImagePlugin::Initialize(QGLWidget* canvas)
234  {
235  canvas_ = canvas;
236 
237  return true;
238  }
239 
240  void RobotImagePlugin::Draw(double x, double y, double scale)
241  {
243  {
244  glColor3f(1.0f, 1.0f, 1.0f);
245  glEnable(GL_TEXTURE_2D);
246  glBindTexture(GL_TEXTURE_2D, static_cast<GLuint>(texture_id_));
247 
248  glBegin(GL_QUADS);
249 
250  glTexCoord2f(0, 1); glVertex2d(top_left_transformed_.x(), top_left_transformed_.y());
251  glTexCoord2f(1, 1); glVertex2d(top_right_transformed_.x(), top_right_transformed_.y());
252  glTexCoord2f(1, 0); glVertex2d(bottom_right_transformed_.x(), bottom_right_transformed_.y());
253  glTexCoord2f(0, 0); glVertex2d(bottom_left_transformed_.x(), bottom_left_transformed_.y());
254 
255  glEnd();
256 
257  glDisable(GL_TEXTURE_2D);
258 
259  PrintInfo("OK");
260  }
261  }
262 
264  {
265  transformed_ = false;
266 
268  if (GetTransform(ros::Time(), transform))
269  {
270  top_left_transformed_ = transform * top_left_;
271  top_right_transformed_ = transform * top_right_;
274  transformed_ = true;
275  }
276  else
277  {
278  PrintError("No transform between " + source_frame_ + " and " + target_frame_);
279  }
280  }
281 
283  {
284  ROS_INFO("Loading image");
285  try
286  {
287  QImage nullImage;
288  image_ = nullImage;
289 
290  if (texture_loaded_)
291  {
292  GLuint ids[1];
293  ids[0] = static_cast<GLuint>(texture_id_);
294  glDeleteTextures(1, &ids[0]);
295  texture_loaded_ = false;
296  }
297 
298  const std::string prefix = "$(find ";
299  std::string real_filename;
300  size_t spos = filename_.find(prefix);
301  bool has_close = spos != -1 ? filename_.find(')', spos) != -1: false;
302  if (spos != -1 && spos + prefix.length() < filename_.size() && has_close)
303  {
304  std::string package = filename_.substr(spos + prefix.length());
305  package = package.substr(0, package.find(")"));
306 
307  real_filename = ros::package::getPath(package) + filename_.substr(filename_.find(')')+1);
308  }
309  else
310  {
311  real_filename = filename_;
312  }
313 
314 
315  if (image_.load(real_filename.c_str()))
316  {
317  int width = image_.width();
318  int height = image_.height();
319  image_ratio_ = (double)height / (double)width;
320 
321  float max_dim = std::max(width, height);
322  dimension_ = static_cast<int>(std::pow(2, std::ceil(std::log(max_dim) / std::log(2.0f))));
323 
324  if (width != dimension_ || height != dimension_)
325  {
326  image_ = image_.scaled(dimension_, dimension_, Qt::IgnoreAspectRatio, Qt::FastTransformation);
327  }
328 
329  image_ = QGLWidget::convertToGLFormat(image_);
330 
331  GLuint ids[1];
332  glGenTextures(1, &ids[0]);
333  texture_id_ = ids[0];
334 
335  glBindTexture(GL_TEXTURE_2D, static_cast<GLuint>(texture_id_));
336  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, dimension_, dimension_, 0, GL_RGBA, GL_UNSIGNED_BYTE, image_.bits());
337 
338  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
339  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
340 
341  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
342  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
343 
344  texture_loaded_ = true;
345  if( ui_.ratio_original->isChecked() )
346  {
347  RatioOriginalToggled(true);
348  }
349  }
350  else
351  {
352  PrintError("Failed to load image.");
353  }
354  }
355  catch (const std::exception& e)
356  {
357  PrintError("Failed to load image. Exception occured.");
358  }
359  }
360 
361  void RobotImagePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
362  {
363  if (node["frame"])
364  {
365  node["frame"] >> source_frame_;
366  ui_.frame->setText(source_frame_.c_str());
367  }
368  if (node["offset_x"])
369  {
370  node["offset_x"] >> offset_x_;
371  ui_.offset_x->setValue(offset_x_);
372  }
373 
374  if (node["offset_y"])
375  {
376  node["offset_y"] >> offset_y_;
377  ui_.offset_y->setValue(offset_y_);
378  }
379 
380  if (node["image"])
381  {
382  node["image"] >> filename_;
383  ui_.image->setText(filename_.c_str());
384  }
385 
386  if (node["width"])
387  {
388  node["width"] >> width_;
389  ui_.width->setValue(width_);
390  }
391 
392  if (node["height"])
393  {
394  node["height"] >> height_;
395  ui_.height->setValue(height_);
396  }
397 
398  if (node["ratio"])
399  {
400  std::string value;
401  node["ratio"] >> value;
402  if(value == "equal")
403  {
404  ui_.ratio_equal->setChecked(true);
405  }
406  else if(value == "custom")
407  {
408  ui_.ratio_custom->setChecked(true);
409  }
410  else if(value == "original")
411  {
412  ui_.ratio_original->setChecked(true);
413  }
414  }
415 
416  UpdateShape();
417  LoadImage();
418  FrameEdited();
419  }
420 
421  void RobotImagePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
422  {
423  emitter << YAML::Key << "frame" << YAML::Value << ui_.frame->text().toStdString();
424  emitter << YAML::Key << "image" << YAML::Value << ui_.image->text().toStdString();
425  emitter << YAML::Key << "width" << YAML::Value << width_;
426  emitter << YAML::Key << "height" << YAML::Value << height_;
427  emitter << YAML::Key << "offset_x" << YAML::Value << offset_x_;
428  emitter << YAML::Key << "offset_y" << YAML::Value << offset_y_;
429  if( ui_.ratio_custom->isChecked())
430  {
431  emitter << YAML::Key << "ratio" << YAML::Value << "custom";
432  }
433  else if( ui_.ratio_equal->isChecked())
434  {
435  emitter << YAML::Key << "ratio" << YAML::Value << "equal";
436  }
437  else if( ui_.ratio_original->isChecked())
438  {
439  emitter << YAML::Key << "ratio" << YAML::Value << "original";
440  }
441  }
442 }
443 
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
void PrintWarning(const std::string &message)
string package
f
void PrintError(const std::string &message)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
QWidget * GetConfigWidget(QWidget *parent)
std::string target_frame_
void PrintInfo(const std::string &message)
static std::string selectFrame(boost::shared_ptr< tf::TransformListener > tf_listener, QWidget *parent=0)
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void Draw(double x, double y, double scale)
std::string source_frame_
#define ROS_INFO(...)
ROSLIB_DECL std::string getPath(const std::string &package_name)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
boost::shared_ptr< tf::TransformListener > tf_
void LoadConfig(const YAML::Node &node, const std::string &path)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Dec 16 2022 03:59:33