DisparityPlugin::DisparityPlugin() : config_widget_(new QWidget()), anchor_(TOP_LEFT), units_(PIXELS), offset_x_(0), offset_y_(0), width_(320), height_(240), has_image_(false), last_width_(0), last_height_(0) { ui_.setupUi(config_widget_); // Set background white QPalette p(config_widget_->palette()); p.setColor(QPalette::Background, Qt::white); config_widget_->setPalette(p); // Set status text red QPalette p3(ui_.status->palette()); p3.setColor(QPalette::Text, Qt::red); ui_.status->setPalette(p3); QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); QObject::connect(ui_.anchor, SIGNAL(activated(QString)), this, SLOT(SetAnchor(QString))); QObject::connect(ui_.units, SIGNAL(activated(QString)), this, SLOT(SetUnits(QString))); QObject::connect(ui_.offsetx, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetX(int))); QObject::connect(ui_.offsety, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetY(int))); QObject::connect(ui_.width, SIGNAL(valueChanged(int)), this, SLOT(SetWidth(int))); QObject::connect(ui_.height, SIGNAL(valueChanged(int)), this, SLOT(SetHeight(int))); }
void DisparityPlugin::SelectTopic() { QDialog dialog; Ui::topicselect ui; ui.setupUi(&dialog); std::vector<ros::master::TopicInfo> topics; ros::master::getTopics(topics); for (unsigned int i = 0; i < topics.size(); i++) { if (topics[i].datatype == "stereo_msgs/DisparityImage") { ui.displaylist->addItem(topics[i].name.c_str()); } } ui.displaylist->setCurrentRow(0); dialog.exec(); if (dialog.result() == QDialog::Accepted && ui.displaylist->selectedItems().count() == 1) { ui_.topic->setText(ui.displaylist->selectedItems().first()->text()); TopicEdited(); } }
void DisparityPlugin::LoadConfig(const YAML::Node& node, const std::string& path) { std::string topic; node["topic"] >> topic; ui_.topic->setText(topic.c_str()); TopicEdited(); std::string anchor; node["anchor"] >> anchor; ui_.anchor->setCurrentIndex(ui_.anchor->findText(anchor.c_str())); SetAnchor(anchor.c_str()); std::string units; node["units"] >> units; ui_.units->setCurrentIndex(ui_.units->findText(units.c_str())); SetUnits(units.c_str()); node["offset_x"] >> offset_x_; ui_.offsetx->setValue(offset_x_); node["offset_y"] >> offset_y_; ui_.offsety->setValue(offset_y_); node["width"] >> width_; ui_.width->setValue(width_); node["height"] >> height_; ui_.height->setValue(height_); }
void LaserScanPlugin::LoadConfig(const YAML::Node& node, const std::string& path) { std::string topic; node["topic"] >> topic; ui_.topic->setText(boost::trim_copy(topic).c_str()); std::string min_color; node["min_color"] >> min_color; min_color_ = QColor(min_color.c_str()); ui_.selectMinColor->setStyleSheet("background: " + min_color_.name() + ";"); std::string max_color; node["max_color"] >> max_color; max_color_ = QColor(max_color.c_str()); ui_.selectMaxColor->setStyleSheet("background: " + max_color_.name() + ";"); node["min_intensity"] >> min_intensity_; ui_.minIntensity->setValue(min_intensity_); node["max_intensity"] >> max_intensity_; ui_.maxIntensity->setValue(max_intensity_); node["point_size"] >> point_size_; ui_.pointSize->setValue(point_size_); node["buffer_size"] >> buffer_size_; ui_.bufferSize->setValue(buffer_size_); TopicEdited(); }
LaserScanPlugin::LaserScanPlugin() : config_widget_(new QWidget()), min_color_(Qt::white), max_color_(Qt::white), min_intensity_(0.0), max_intensity_(100.0), point_size_(3), buffer_size_(1) { ui_.setupUi(config_widget_); // Set background white QPalette p(config_widget_->palette()); p.setColor(QPalette::Background, Qt::white); config_widget_->setPalette(p); // Set status text red QPalette p3(ui_.status->palette()); p3.setColor(QPalette::Text, Qt::red); ui_.status->setPalette(p3); // Initialize color selector colors ui_.selectMinColor->setStyleSheet("background: " + min_color_.name() + ";"); ui_.selectMaxColor->setStyleSheet("background: " + max_color_.name() + ";"); QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); QObject::connect(ui_.selectMinColor, SIGNAL(clicked()), this, SLOT(SelectMinColor())); QObject::connect(ui_.selectMaxColor, SIGNAL(clicked()), this, SLOT(SelectMaxColor())); QObject::connect(ui_.minIntensity, SIGNAL(valueChanged(double)), this, SLOT(MinIntensityChanged(double))); QObject::connect(ui_.maxIntensity, SIGNAL(valueChanged(double)), this, SLOT(MaxIntensityChanged(double))); QObject::connect(ui_.bufferSize, SIGNAL(valueChanged(int)), this, SLOT(BufferSizeChanged(int))); QObject::connect(ui_.pointSize, SIGNAL(valueChanged(int)), this, SLOT(PointSizeChanged(int))); }
AttitudeIndicatorPlugin::AttitudeIndicatorPlugin() : config_widget_(new QWidget()) { ui_.setupUi(config_widget_); // Set background white QPalette p(config_widget_->palette()); p.setColor(QPalette::Background, Qt::white); config_widget_->setPalette(p); roll_ = pitch_ = yaw_ = 0; topics_.push_back("nav_msgs/Odometry"); topics_.push_back("geometry_msgs/Pose"); topics_.push_back("sensor_msgs/Imu"); // Set status text red QPalette p3(ui_.status->palette()); p3.setColor(QPalette::Text, Qt::red); ui_.status->setPalette(p3); placer_.setRect(QRect(0, 0, 100, 100)); QObject::connect(this, SIGNAL(VisibleChanged(bool)), &placer_, SLOT(setVisible(bool))); QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); }
RoutePlugin::RoutePlugin() : config_widget_(new QWidget()), draw_style_(LINES) { ui_.setupUi(config_widget_); ui_.color->setColor(Qt::green); // Set background white QPalette p(config_widget_->palette()); p.setColor(QPalette::Background, Qt::white); config_widget_->setPalette(p); // Set status text red QPalette p3(ui_.status->palette()); p3.setColor(QPalette::Text, Qt::red); ui_.status->setPalette(p3); QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); QObject::connect(ui_.selectpositiontopic, SIGNAL(clicked()), this, SLOT(SelectPositionTopic())); QObject::connect(ui_.positiontopic, SIGNAL(editingFinished()), this, SLOT(PositionTopicEdited())); QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, SLOT(SetDrawStyle(QString))); QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, SLOT(DrawIcon())); }
void AttitudeIndicatorPlugin::SelectTopic() { ros::master::TopicInfo topic = mapviz::SelectTopicDialog::selectTopic( topics_); if (topic.name.empty()) { return; } ui_.topic->setText(QString::fromStdString(topic.name)); TopicEdited(); }
void RoutePlugin::SelectTopic() { ros::master::TopicInfo topic = mapviz::SelectTopicDialog::selectTopic("marti_nav_msgs/Route"); if (topic.name.empty()) { return; } ui_.topic->setText(QString::fromStdString(topic.name)); TopicEdited(); }
void RoutePlugin::LoadConfig(const YAML::Node& node, const std::string& path) { if (node["topic"]) { std::string route_topic; node["topic"] >> route_topic; ui_.topic->setText(route_topic.c_str()); } if (node["color"]) { std::string color; node["color"] >> color; ui_.color->setColor(QColor(color.c_str())); } if (node["postopic"]) { std::string pos_topic; node["postopic"] >> pos_topic; ui_.positiontopic->setText(pos_topic.c_str()); } if (node["poscolor"]) { std::string poscolor; node["poscolor"] >> poscolor; ui_.positioncolor->setColor(QColor(poscolor.c_str())); } if (node["draw_style"]) { std::string draw_style; node["draw_style"] >> draw_style; if (draw_style == "lines") { draw_style_ = LINES; ui_.drawstyle->setCurrentIndex(0); } else if (draw_style == "points") { draw_style_ = POINTS; ui_.drawstyle->setCurrentIndex(1); } } TopicEdited(); PositionTopicEdited(); }