00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <mapviz_plugins/route_plugin.h>
00031
00032
00033 #include <cstdio>
00034 #include <vector>
00035
00036
00037 #include <QDialog>
00038 #include <QGLWidget>
00039 #include <QPainter>
00040 #include <QPalette>
00041
00042 #include <opencv2/core/core.hpp>
00043
00044
00045 #include <ros/master.h>
00046
00047 #include <swri_image_util/geometry_util.h>
00048 #include <swri_route_util/util.h>
00049 #include <swri_transform_util/transform_util.h>
00050 #include <mapviz/select_topic_dialog.h>
00051
00052 #include <marti_nav_msgs/Route.h>
00053
00054
00055 #include <pluginlib/class_list_macros.h>
00056
00057 PLUGINLIB_EXPORT_CLASS(mapviz_plugins::RoutePlugin, mapviz::MapvizPlugin)
00058
00059 namespace sru = swri_route_util;
00060 namespace stu = swri_transform_util;
00061
00062 namespace mapviz_plugins
00063 {
00064 RoutePlugin::RoutePlugin() : config_widget_(new QWidget()), draw_style_(LINES)
00065 {
00066 ui_.setupUi(config_widget_);
00067
00068 ui_.color->setColor(Qt::green);
00069
00070 QPalette p(config_widget_->palette());
00071 p.setColor(QPalette::Background, Qt::white);
00072 config_widget_->setPalette(p);
00073
00074 QPalette p3(ui_.status->palette());
00075 p3.setColor(QPalette::Text, Qt::red);
00076 ui_.status->setPalette(p3);
00077 QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
00078 SLOT(SelectTopic()));
00079 QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
00080 SLOT(TopicEdited()));
00081 QObject::connect(ui_.selectpositiontopic, SIGNAL(clicked()), this,
00082 SLOT(SelectPositionTopic()));
00083 QObject::connect(ui_.positiontopic, SIGNAL(editingFinished()), this,
00084 SLOT(PositionTopicEdited()));
00085 QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
00086 SLOT(SetDrawStyle(QString)));
00087 QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
00088 SLOT(DrawIcon()));
00089 }
00090
00091 RoutePlugin::~RoutePlugin()
00092 {
00093 }
00094
00095 void RoutePlugin::DrawIcon()
00096 {
00097 if (icon_)
00098 {
00099 QPixmap icon(16, 16);
00100 icon.fill(Qt::transparent);
00101
00102 QPainter painter(&icon);
00103 painter.setRenderHint(QPainter::Antialiasing, true);
00104
00105 QPen pen(ui_.color->color());
00106
00107 if (draw_style_ == POINTS)
00108 {
00109 pen.setWidth(7);
00110 pen.setCapStyle(Qt::RoundCap);
00111 painter.setPen(pen);
00112 painter.drawPoint(8, 8);
00113 }
00114 else if (draw_style_ == LINES)
00115 {
00116 pen.setWidth(3);
00117 pen.setCapStyle(Qt::FlatCap);
00118 painter.setPen(pen);
00119 painter.drawLine(1, 14, 14, 1);
00120 }
00121
00122 icon_->SetPixmap(icon);
00123 }
00124 }
00125
00126 void RoutePlugin::SetDrawStyle(QString style)
00127 {
00128 if (style == "lines")
00129 {
00130 draw_style_ = LINES;
00131 }
00132 else if (style == "points")
00133 {
00134 draw_style_ = POINTS;
00135 }
00136 DrawIcon();
00137 }
00138
00139 void RoutePlugin::SelectTopic()
00140 {
00141 ros::master::TopicInfo topic =
00142 mapviz::SelectTopicDialog::selectTopic("marti_nav_msgs/Route");
00143
00144 if (topic.name.empty())
00145 {
00146 return;
00147 }
00148
00149 ui_.topic->setText(QString::fromStdString(topic.name));
00150 TopicEdited();
00151 }
00152
00153 void RoutePlugin::SelectPositionTopic()
00154 {
00155 ros::master::TopicInfo topic =
00156 mapviz::SelectTopicDialog::selectTopic("marti_nav_msgs/RoutePosition");
00157
00158 if (topic.name.empty())
00159 {
00160 return;
00161 }
00162
00163 ui_.positiontopic->setText(QString::fromStdString(topic.name));
00164 PositionTopicEdited();
00165 }
00166
00167 void RoutePlugin::TopicEdited()
00168 {
00169 std::string topic = ui_.topic->text().trimmed().toStdString();
00170 if (topic != topic_)
00171 {
00172 src_route_ = sru::Route();
00173
00174 route_sub_.shutdown();
00175
00176 topic_ = topic;
00177 if (!topic.empty())
00178 {
00179 route_sub_ =
00180 node_.subscribe(topic_, 1, &RoutePlugin::RouteCallback, this);
00181
00182 ROS_INFO("Subscribing to %s", topic_.c_str());
00183 }
00184 }
00185 }
00186
00187 void RoutePlugin::PositionTopicEdited()
00188 {
00189 std::string topic = ui_.positiontopic->text().trimmed().toStdString();
00190 if (topic != position_topic_)
00191 {
00192 src_route_position_.reset();
00193 position_sub_.shutdown();
00194
00195 if (!topic.empty())
00196 {
00197 position_topic_ = topic;
00198 position_sub_ = node_.subscribe(position_topic_, 1,
00199 &RoutePlugin::PositionCallback, this);
00200
00201 ROS_INFO("Subscribing to %s", position_topic_.c_str());
00202 }
00203 }
00204 }
00205
00206 void RoutePlugin::PositionCallback(
00207 const marti_nav_msgs::RoutePositionConstPtr& msg)
00208 {
00209 src_route_position_ = msg;
00210 }
00211
00212 void RoutePlugin::RouteCallback(const marti_nav_msgs::RouteConstPtr& msg)
00213 {
00214 src_route_ = sru::Route(*msg);
00215 }
00216
00217 void RoutePlugin::PrintError(const std::string& message)
00218 {
00219 PrintErrorHelper(ui_.status, message, 1.0);
00220 }
00221
00222 void RoutePlugin::PrintInfo(const std::string& message)
00223 {
00224 PrintInfoHelper(ui_.status, message, 1.0);
00225 }
00226
00227 void RoutePlugin::PrintWarning(const std::string& message)
00228 {
00229 PrintWarningHelper(ui_.status, message, 1.0);
00230 }
00231
00232 QWidget* RoutePlugin::GetConfigWidget(QWidget* parent)
00233 {
00234 config_widget_->setParent(parent);
00235
00236 return config_widget_;
00237 }
00238
00239 bool RoutePlugin::Initialize(QGLWidget* canvas)
00240 {
00241 canvas_ = canvas;
00242
00243 DrawIcon();
00244
00245 initialized_ = true;
00246 return true;
00247 }
00248
00249 void RoutePlugin::Draw(double x, double y, double scale)
00250 {
00251 if (!src_route_.valid())
00252 {
00253 PrintError("No valid route received.");
00254 return;
00255 }
00256
00257 sru::Route route = src_route_;
00258 if (route.header.frame_id.empty())
00259 {
00260 route.header.frame_id = "/wgs84";
00261 }
00262
00263 stu::Transform transform;
00264 if (!GetTransform(route.header.frame_id, ros::Time(), transform))
00265 {
00266 PrintError("Failed to transform route");
00267 return;
00268 }
00269
00270 sru::transform(route, transform, target_frame_);
00271 sru::projectToXY(route);
00272 sru::fillOrientations(route);
00273
00274 DrawRoute(route);
00275
00276 bool ok = true;
00277 if (route.valid() && src_route_position_)
00278 {
00279 sru::RoutePoint point;
00280 if (sru::interpolateRoutePosition(point, route, *src_route_position_, true))
00281 {
00282 DrawRoutePoint(point);
00283 }
00284 else
00285 {
00286 PrintError("Failed to find route position in route.");
00287 ok = false;
00288 }
00289 }
00290
00291 if (ok)
00292 {
00293 PrintInfo("OK");
00294 }
00295 }
00296
00297 void RoutePlugin::DrawStopWaypoint(double x, double y)
00298 {
00299 const double a = 2;
00300 const double S = a * 2.414213562373095;
00301
00302 glBegin(GL_POLYGON);
00303
00304 glColor3f(1.0, 0.0, 0.0);
00305
00306 glVertex2d(x + S / 2.0, y - a / 2.0);
00307 glVertex2d(x + S / 2.0, y + a / 2.0);
00308 glVertex2d(x + a / 2.0, y + S / 2.0);
00309 glVertex2d(x - a / 2.0, y + S / 2.0);
00310 glVertex2d(x - S / 2.0, y + a / 2.0);
00311 glVertex2d(x - S / 2.0, y - a / 2.0);
00312 glVertex2d(x - a / 2.0, y - S / 2.0);
00313 glVertex2d(x + a / 2.0, y - S / 2.0);
00314
00315 glEnd();
00316 }
00317
00318 void RoutePlugin::DrawRoute(const sru::Route& route)
00319 {
00320 const QColor color = ui_.color->color();
00321 glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0);
00322
00323 if (draw_style_ == LINES)
00324 {
00325 glLineWidth(3);
00326 glBegin(GL_LINE_STRIP);
00327 }
00328 else
00329 {
00330 glPointSize(2);
00331 glBegin(GL_POINTS);
00332 }
00333
00334 for (size_t i = 0; i < route.points.size(); i++)
00335 {
00336 glVertex2d(route.points[i].position().x(),
00337 route.points[i].position().y());
00338 }
00339 glEnd();
00340 }
00341
00342 void RoutePlugin::DrawRoutePoint(const sru::RoutePoint& point)
00343 {
00344 const double arrow_size = ui_.iconsize->value();
00345
00346 tf::Vector3 v1(arrow_size, 0.0, 0.0);
00347 tf::Vector3 v2(0.0, arrow_size / 2.0, 0.0);
00348 tf::Vector3 v3(0.0, -arrow_size / 2.0, 0.0);
00349
00350 tf::Transform point_g(point.orientation(), point.position());
00351
00352 v1 = point_g * v1;
00353 v2 = point_g * v2;
00354 v3 = point_g * v3;
00355
00356 const QColor color = ui_.positioncolor->color();
00357 glLineWidth(3);
00358 glBegin(GL_POLYGON);
00359 glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0);
00360 glVertex2d(v1.x(), v1.y());
00361 glVertex2d(v2.x(), v2.y());
00362 glVertex2d(v3.x(), v3.y());
00363 glEnd();
00364 }
00365
00366 void RoutePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
00367 {
00368 if (node["topic"])
00369 {
00370 std::string route_topic;
00371 node["topic"] >> route_topic;
00372 ui_.topic->setText(route_topic.c_str());
00373 }
00374 if (node["color"])
00375 {
00376 std::string color;
00377 node["color"] >> color;
00378 ui_.color->setColor(QColor(color.c_str()));
00379 }
00380 if (node["postopic"])
00381 {
00382 std::string pos_topic;
00383 node["postopic"] >> pos_topic;
00384 ui_.positiontopic->setText(pos_topic.c_str());
00385 }
00386 if (node["poscolor"])
00387 {
00388 std::string poscolor;
00389 node["poscolor"] >> poscolor;
00390 ui_.positioncolor->setColor(QColor(poscolor.c_str()));
00391 }
00392
00393 if (node["draw_style"])
00394 {
00395 std::string draw_style;
00396 node["draw_style"] >> draw_style;
00397
00398 if (draw_style == "lines")
00399 {
00400 draw_style_ = LINES;
00401 ui_.drawstyle->setCurrentIndex(0);
00402 }
00403 else if (draw_style == "points")
00404 {
00405 draw_style_ = POINTS;
00406 ui_.drawstyle->setCurrentIndex(1);
00407 }
00408 }
00409
00410 TopicEdited();
00411 PositionTopicEdited();
00412 }
00413
00414 void RoutePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
00415 {
00416 std::string route_topic = ui_.topic->text().toStdString();
00417 emitter << YAML::Key << "topic" << YAML::Value << route_topic;
00418
00419 std::string color = ui_.color->color().name().toStdString();
00420 emitter << YAML::Key << "color" << YAML::Value << color;
00421
00422 std::string pos_topic = ui_.positiontopic->text().toStdString();
00423 emitter << YAML::Key << "postopic" << YAML::Value << pos_topic;
00424
00425 std::string pos_color = ui_.positioncolor->color().name().toStdString();
00426 emitter << YAML::Key << "poscolor" << YAML::Value << pos_color;
00427
00428 std::string draw_style = ui_.drawstyle->currentText().toStdString();
00429 emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
00430 }
00431 }