00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <mapviz_plugins/navsat_plugin.h>
00021
00022
00023 #include <cstdio>
00024 #include <vector>
00025
00026
00027 #include <QDialog>
00028 #include <QGLWidget>
00029 #include <QPalette>
00030
00031 #include <opencv2/core/core.hpp>
00032
00033
00034 #include <ros/master.h>
00035
00036 #include <swri_image_util/geometry_util.h>
00037 #include <swri_transform_util/transform_util.h>
00038
00039 #include <mapviz/select_topic_dialog.h>
00040
00041
00042 #include <pluginlib/class_list_macros.h>
00043 PLUGINLIB_DECLARE_CLASS(mapviz_plugins,
00044 navsat,
00045 mapviz_plugins::NavSatPlugin,
00046 mapviz::MapvizPlugin);
00047
00048 namespace mapviz_plugins
00049 {
00050 NavSatPlugin::NavSatPlugin() : config_widget_(new QWidget())
00051 {
00052 ui_.setupUi(config_widget_);
00053
00054 ui_.color->setColor(Qt::green);
00055
00056
00057 QPalette p(config_widget_->palette());
00058 p.setColor(QPalette::Background, Qt::white);
00059 config_widget_->setPalette(p);
00060
00061
00062 QPalette p3(ui_.status->palette());
00063 p3.setColor(QPalette::Text, Qt::red);
00064 ui_.status->setPalette(p3);
00065
00066 QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
00067 SLOT(SelectTopic()));
00068 QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
00069 SLOT(TopicEdited()));
00070 QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
00071 SLOT(PositionToleranceChanged(double)));
00072 QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this,
00073 SLOT(BufferSizeChanged(int)));
00074 QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
00075 SLOT(SetDrawStyle(QString)));
00076 QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)),
00077 this, SLOT(SetStaticArrowSizes(bool)));
00078 QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
00079 this, SLOT(SetArrowSize(int)));
00080 connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
00081 SLOT(SetColor(const QColor&)));
00082 }
00083
00084 NavSatPlugin::~NavSatPlugin()
00085 {
00086 }
00087
00088 void NavSatPlugin::SelectTopic()
00089 {
00090 ros::master::TopicInfo topic =
00091 mapviz::SelectTopicDialog::selectTopic("sensor_msgs/NavSatFix");
00092
00093 if (!topic.name.empty())
00094 {
00095 ui_.topic->setText(QString::fromStdString(topic.name));
00096 TopicEdited();
00097 }
00098 }
00099
00100 void NavSatPlugin::TopicEdited()
00101 {
00102 std::string topic = ui_.topic->text().trimmed().toStdString();
00103 if (topic != topic_)
00104 {
00105 initialized_ = false;
00106 points_.clear();
00107 has_message_ = false;
00108 PrintWarning("No messages received.");
00109
00110 navsat_sub_.shutdown();
00111 topic_ = topic;
00112 if (!topic.empty())
00113 {
00114 navsat_sub_ = node_.subscribe(topic_, 1, &NavSatPlugin::NavSatFixCallback, this);
00115
00116 ROS_INFO("Subscribing to %s", topic_.c_str());
00117 }
00118 }
00119 }
00120
00121 void NavSatPlugin::NavSatFixCallback(
00122 const sensor_msgs::NavSatFixConstPtr navsat)
00123 {
00124 if (!local_xy_util_.Initialized())
00125 {
00126 return;
00127 }
00128 if (!has_message_)
00129 {
00130 initialized_ = true;
00131 has_message_ = true;
00132 }
00133
00134 StampedPoint stamped_point;
00135 stamped_point.stamp = navsat->header.stamp;
00136
00137 double x;
00138 double y;
00139 local_xy_util_.ToLocalXy(navsat->latitude, navsat->longitude, x, y);
00140
00141 stamped_point.point = tf::Point(x, y, navsat->altitude);
00142
00143 stamped_point.orientation = tf::createQuaternionFromYaw(0.0);
00144
00145 stamped_point.source_frame = local_xy_util_.Frame();
00146
00147 if (points_.empty() ||
00148 stamped_point.point.distance(points_.back().point) >=
00149 position_tolerance_)
00150 {
00151 points_.push_back(stamped_point);
00152 }
00153
00154 if (buffer_size_ > 0)
00155 {
00156 while (static_cast<int>(points_.size()) > buffer_size_)
00157 {
00158 points_.pop_front();
00159 }
00160 }
00161
00162 cur_point_ = stamped_point;
00163 }
00164
00165 void NavSatPlugin::PrintError(const std::string& message)
00166 {
00167 if (message == ui_.status->text().toStdString())
00168 {
00169 return;
00170 }
00171
00172 ROS_ERROR("Error: %s", message.c_str());
00173 QPalette p(ui_.status->palette());
00174 p.setColor(QPalette::Text, Qt::red);
00175 ui_.status->setPalette(p);
00176 ui_.status->setText(message.c_str());
00177 }
00178
00179 void NavSatPlugin::PrintInfo(const std::string& message)
00180 {
00181 if (message == ui_.status->text().toStdString())
00182 {
00183 return;
00184 }
00185
00186 ROS_INFO("%s", message.c_str());
00187 QPalette p(ui_.status->palette());
00188 p.setColor(QPalette::Text, Qt::green);
00189 ui_.status->setPalette(p);
00190 ui_.status->setText(message.c_str());
00191 }
00192
00193 void NavSatPlugin::PrintWarning(const std::string& message)
00194 {
00195 if (message == ui_.status->text().toStdString())
00196 {
00197 return;
00198 }
00199
00200 ROS_WARN("%s", message.c_str());
00201 QPalette p(ui_.status->palette());
00202 p.setColor(QPalette::Text, Qt::darkYellow);
00203 ui_.status->setPalette(p);
00204 ui_.status->setText(message.c_str());
00205 }
00206
00207 QWidget* NavSatPlugin::GetConfigWidget(QWidget* parent)
00208 {
00209 config_widget_->setParent(parent);
00210
00211 return config_widget_;
00212 }
00213
00214 bool NavSatPlugin::Initialize(QGLWidget* canvas)
00215 {
00216 canvas_ = canvas;
00217 SetColor(ui_.color->color());
00218 return true;
00219 }
00220
00221 void NavSatPlugin::Draw(double x, double y, double scale)
00222 {
00223 if (DrawPoints(scale))
00224 {
00225 PrintInfo("OK");
00226 }
00227 }
00228
00229 void NavSatPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
00230 {
00231 if (node["topic"])
00232 {
00233 std::string topic;
00234 node["topic"] >> topic;
00235 ui_.topic->setText(topic.c_str());
00236 }
00237
00238 if (node["color"])
00239 {
00240 std::string color;
00241 node["color"] >> color;
00242 SetColor(QColor(color.c_str()));
00243 ui_.color->setColor(color_);
00244 }
00245
00246 if (node["draw_style"])
00247 {
00248 std::string draw_style;
00249 node["draw_style"] >> draw_style;
00250
00251 if (draw_style == "lines")
00252 {
00253 draw_style_ = LINES;
00254 ui_.drawstyle->setCurrentIndex(0);
00255 }
00256 else if (draw_style == "points")
00257 {
00258 draw_style_ = POINTS;
00259 ui_.drawstyle->setCurrentIndex(1);
00260 }
00261 }
00262
00263 if (node["position_tolerance"])
00264 {
00265 node["position_tolerance"] >> position_tolerance_;
00266 ui_.positiontolerance->setValue(position_tolerance_);
00267 }
00268
00269 if (node["buffer_size"])
00270 {
00271 node["buffer_size"] >> buffer_size_;
00272 ui_.buffersize->setValue(buffer_size_);
00273 }
00274
00275 if (node["static_arrow_sizes"])
00276 {
00277 bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
00278 ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
00279 SetStaticArrowSizes(static_arrow_sizes);
00280 }
00281
00282 if (node["arrow_size"])
00283 {
00284 ui_.arrow_size->setValue(node["arrow_size"].as<int>());
00285 }
00286
00287 TopicEdited();
00288 }
00289
00290 void NavSatPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
00291 {
00292 std::string topic = ui_.topic->text().toStdString();
00293 emitter << YAML::Key << "topic" << YAML::Value << topic;
00294
00295 std::string color = ui_.color->color().name().toStdString();
00296 emitter << YAML::Key << "color" << YAML::Value << color;
00297
00298 std::string draw_style = ui_.drawstyle->currentText().toStdString();
00299 emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
00300
00301 emitter << YAML::Key << "position_tolerance" <<
00302 YAML::Value << position_tolerance_;
00303
00304 emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_;
00305
00306 emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
00307
00308 emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
00309 }
00310 }