43 #include <opencv2/imgproc/imgproc.hpp>
54 config_widget_(new QWidget()),
61 transport_(
"default"),
65 original_aspect_ratio_(1.0)
71 p.setColor(QPalette::Background, Qt::white);
75 QPalette p3(
ui_.status->palette());
76 p3.setColor(QPalette::Text, Qt::red);
77 ui_.status->setPalette(p3);
79 QObject::connect(
ui_.selecttopic, SIGNAL(clicked()),
this, SLOT(
SelectTopic()));
80 QObject::connect(
ui_.topic, SIGNAL(editingFinished()),
this, SLOT(
TopicEdited()));
81 QObject::connect(
ui_.anchor, SIGNAL(activated(QString)),
this, SLOT(
SetAnchor(QString)));
82 QObject::connect(
ui_.units, SIGNAL(activated(QString)),
this, SLOT(
SetUnits(QString)));
83 QObject::connect(
ui_.offsetx, SIGNAL(valueChanged(
int)),
this, SLOT(
SetOffsetX(
int)));
84 QObject::connect(
ui_.offsety, SIGNAL(valueChanged(
int)),
this, SLOT(
SetOffsetY(
int)));
85 QObject::connect(
ui_.width, SIGNAL(valueChanged(
double)),
this, SLOT(
SetWidth(
double)));
86 QObject::connect(
ui_.height, SIGNAL(valueChanged(
double)),
this, SLOT(
SetHeight(
double)));
88 QObject::connect(
ui_.keep_ratio, SIGNAL(toggled(
bool)),
this, SLOT(
KeepRatioChanged(
bool)));
89 QObject::connect(
ui_.transport_combo_box, SIGNAL(activated(
const QString&)),
92 ui_.width->setKeyboardTracking(
false);
93 ui_.height->setKeyboardTracking(
false);
122 if (anchor ==
"top left")
126 else if (anchor ==
"top center")
130 else if (anchor ==
"top right")
134 else if (anchor ==
"center left")
138 else if (anchor ==
"center")
142 else if (anchor ==
"center right")
146 else if (anchor ==
"bottom left")
150 else if (anchor ==
"bottom center")
154 else if (anchor ==
"bottom right")
163 ui_.width->setMaximum(10000);
164 ui_.height->setMaximum(10000);
166 if (units ==
"pixels")
168 ui_.width->setDecimals(0);
169 ui_.height->setDecimals(0);
173 ui_.width->setSuffix(
" px");
174 ui_.height->setSuffix(
" px");
176 else if (units ==
"percent")
178 ui_.width->setDecimals(1);
179 ui_.height->setDecimals(1);
183 ui_.width->setSuffix(
" %");
184 ui_.height->setSuffix(
" %");
191 ui_.width->setMaximum(100);
192 ui_.height->setMaximum(100);
222 ui_.height->setEnabled( !checked );
241 "sensor_msgs/Image");
243 if(topic.
name.empty())
249 if (!topic.
name.empty())
251 ui_.topic->setText(QString::fromStdString(topic.
name));
258 std::string topic =
ui_.topic->text().trimmed().toStdString();
296 ROS_DEBUG(
"Setting transport to %s on %s.",
336 if(
ui_.keep_ratio->isChecked() )
343 ui_.height->setValue(height);
392 if (image ==
NULL || image->cols == 0 || image->rows == 0)
398 switch (image->channels())
401 format = GL_LUMINANCE;
404 format = GL_LUMINANCE_ALPHA;
413 glPixelZoom(1.0
f, -1.0
f);
414 glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
415 glDrawPixels(image->cols, image->rows, format, GL_UNSIGNED_BYTE, image->ptr());
416 glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
438 if(
ui_.keep_ratio->isChecked() )
459 x_pos = (
canvas_->width() - width) / 2.0 + x_offset;
464 x_pos =
canvas_->width() - width - x_offset;
470 y_pos = (
canvas_->height() - height) / 2.0 + y_offset;
474 x_pos = (
canvas_->width() - width) / 2.0 + x_offset;
475 y_pos = (
canvas_->height() - height) / 2.0 + y_offset;
479 x_pos =
canvas_->width() - width - x_offset;
480 y_pos = (
canvas_->height() - height) / 2.0 + y_offset;
485 y_pos =
canvas_->height() - height - y_offset;
489 x_pos = (
canvas_->width() - width) / 2.0 + x_offset;
490 y_pos =
canvas_->height() - height - y_offset;
494 x_pos =
canvas_->width() - width - x_offset;
495 y_pos =
canvas_->height() - height - y_offset;
498 glMatrixMode(GL_PROJECTION);
503 glRasterPos2d(x_pos, y_pos);
518 if (node[
"image_transport"])
521 int index =
ui_.transport_combo_box->findText( QString::fromStdString(
transport_) );
524 ui_.transport_combo_box->setCurrentIndex(index);
528 ROS_WARN(
"Saved image transport %s is unavailable.",
536 node[
"topic"] >> topic;
537 ui_.topic->setText(topic.c_str());
544 node[
"anchor"] >> anchor;
545 ui_.anchor->setCurrentIndex(
ui_.anchor->findText(anchor.c_str()));
552 node[
"units"] >> units;
553 ui_.units->setCurrentIndex(
ui_.units->findText(units.c_str()));
557 if (node[
"offset_x"])
563 if (node[
"offset_y"])
581 if (node[
"keep_ratio"])
584 node[
"keep_ratio"] >> keep;
585 ui_.keep_ratio->setChecked( keep );
591 emitter << YAML::Key <<
"topic" << YAML::Value <<
ui_.topic->text().toStdString();
594 emitter << YAML::Key <<
"offset_x" << YAML::Value <<
offset_x_;
595 emitter << YAML::Key <<
"offset_y" << YAML::Value <<
offset_y_;
596 emitter << YAML::Key <<
"width" << YAML::Value <<
width_;
597 emitter << YAML::Key <<
"height" << YAML::Value <<
height_;
598 emitter << YAML::Key <<
"keep_ratio" << YAML::Value <<
ui_.keep_ratio->isChecked();
599 emitter << YAML::Key <<
"image_transport" << YAML::Value <<
transport_;
604 std::string anchor_string =
"top left";
608 anchor_string =
"top left";
612 anchor_string =
"top center";
616 anchor_string =
"top right";
620 anchor_string =
"center left";
622 else if (anchor ==
CENTER)
624 anchor_string =
"center";
628 anchor_string =
"center right";
632 anchor_string =
"bottom left";
636 anchor_string =
"bottom center";
640 anchor_string =
"bottom right";
643 return anchor_string;
648 std::string units_string =
"pixels";
652 units_string =
"pixels";
656 units_string =
"percent";
669 snprintf(buf,
sizeof(buf),
"image_%llu", (
unsigned long long)
ros::WallTime::now().toNSec());
681 Q_FOREACH (
const std::string& transport, transports)
683 QString qtransport = QString::fromStdString(transport).replace(
"image_transport/",
"");
684 ui_.transport_combo_box->addItem(qtransport);