route_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 <vector>
35 
36 // QT libraries
37 #include <QDialog>
38 #include <QGLWidget>
39 #include <QPainter>
40 #include <QPalette>
41 
42 #include <opencv2/core/core.hpp>
43 
44 // ROS libraries
45 #include <ros/master.h>
46 
48 #include <swri_route_util/util.h>
51 
52 #include <marti_nav_msgs/Route.h>
53 
54 // Declare plugin
56 
58 
59 namespace sru = swri_route_util;
60 namespace stu = swri_transform_util;
61 
62 namespace mapviz_plugins
63 {
64  RoutePlugin::RoutePlugin() : config_widget_(new QWidget()), draw_style_(LINES)
65  {
66  ui_.setupUi(config_widget_);
67 
68  ui_.color->setColor(Qt::green);
69  // Set background white
70  QPalette p(config_widget_->palette());
71  p.setColor(QPalette::Background, Qt::white);
72  config_widget_->setPalette(p);
73  // Set status text red
74  QPalette p3(ui_.status->palette());
75  p3.setColor(QPalette::Text, Qt::red);
76  ui_.status->setPalette(p3);
77  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
78  SLOT(SelectTopic()));
79  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
80  SLOT(TopicEdited()));
81  QObject::connect(ui_.selectpositiontopic, SIGNAL(clicked()), this,
82  SLOT(SelectPositionTopic()));
83  QObject::connect(ui_.positiontopic, SIGNAL(editingFinished()), this,
84  SLOT(PositionTopicEdited()));
85  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
86  SLOT(SetDrawStyle(QString)));
87  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
88  SLOT(DrawIcon()));
89  }
90 
92  {
93  }
94 
96  {
97  if (icon_)
98  {
99  QPixmap icon(16, 16);
100  icon.fill(Qt::transparent);
101 
102  QPainter painter(&icon);
103  painter.setRenderHint(QPainter::Antialiasing, true);
104 
105  QPen pen(ui_.color->color());
106 
107  if (draw_style_ == POINTS)
108  {
109  pen.setWidth(7);
110  pen.setCapStyle(Qt::RoundCap);
111  painter.setPen(pen);
112  painter.drawPoint(8, 8);
113  }
114  else if (draw_style_ == LINES)
115  {
116  pen.setWidth(3);
117  pen.setCapStyle(Qt::FlatCap);
118  painter.setPen(pen);
119  painter.drawLine(1, 14, 14, 1);
120  }
121 
122  icon_->SetPixmap(icon);
123  }
124  }
125 
126  void RoutePlugin::SetDrawStyle(QString style)
127  {
128  if (style == "lines")
129  {
130  draw_style_ = LINES;
131  }
132  else if (style == "points")
133  {
135  }
136  DrawIcon();
137  }
138 
140  {
141  ros::master::TopicInfo topic =
142  mapviz::SelectTopicDialog::selectTopic("marti_nav_msgs/Route");
143 
144  if (topic.name.empty())
145  {
146  return;
147  }
148 
149  ui_.topic->setText(QString::fromStdString(topic.name));
150  TopicEdited();
151  }
152 
154  {
155  ros::master::TopicInfo topic =
156  mapviz::SelectTopicDialog::selectTopic("marti_nav_msgs/RoutePosition");
157 
158  if (topic.name.empty())
159  {
160  return;
161  }
162 
163  ui_.positiontopic->setText(QString::fromStdString(topic.name));
165  }
166 
168  {
169  std::string topic = ui_.topic->text().trimmed().toStdString();
170  if (topic != topic_)
171  {
173 
175 
176  topic_ = topic;
177  if (!topic.empty())
178  {
179  route_sub_ =
181 
182  ROS_INFO("Subscribing to %s", topic_.c_str());
183  }
184  }
185  }
186 
188  {
189  std::string topic = ui_.positiontopic->text().trimmed().toStdString();
190  if (topic != position_topic_)
191  {
192  src_route_position_.reset();
194 
195  if (!topic.empty())
196  {
197  position_topic_ = topic;
200 
201  ROS_INFO("Subscribing to %s", position_topic_.c_str());
202  }
203  }
204  }
205 
207  const marti_nav_msgs::RoutePositionConstPtr& msg)
208  {
209  src_route_position_ = msg;
210  }
211 
212  void RoutePlugin::RouteCallback(const marti_nav_msgs::RouteConstPtr& msg)
213  {
214  src_route_ = sru::Route(*msg);
215  }
216 
217  void RoutePlugin::PrintError(const std::string& message)
218  {
219  PrintErrorHelper(ui_.status, message, 1.0);
220  }
221 
222  void RoutePlugin::PrintInfo(const std::string& message)
223  {
224  PrintInfoHelper(ui_.status, message, 1.0);
225  }
226 
227  void RoutePlugin::PrintWarning(const std::string& message)
228  {
229  PrintWarningHelper(ui_.status, message, 1.0);
230  }
231 
232  QWidget* RoutePlugin::GetConfigWidget(QWidget* parent)
233  {
234  config_widget_->setParent(parent);
235 
236  return config_widget_;
237  }
238 
239  bool RoutePlugin::Initialize(QGLWidget* canvas)
240  {
241  canvas_ = canvas;
242 
243  DrawIcon();
244 
245  initialized_ = true;
246  return true;
247  }
248 
249  void RoutePlugin::Draw(double x, double y, double scale)
250  {
251  if (!src_route_.valid())
252  {
253  PrintError("No valid route received.");
254  return;
255  }
256 
257  sru::Route route = src_route_;
258  if (route.header.frame_id.empty())
259  {
260  route.header.frame_id = "/wgs84";
261  }
262 
264  if (!GetTransform(route.header.frame_id, ros::Time(), transform))
265  {
266  PrintError("Failed to transform route");
267  return;
268  }
269 
270  sru::transform(route, transform, target_frame_);
271  sru::projectToXY(route);
272  sru::fillOrientations(route);
273 
274  DrawRoute(route);
275 
276  bool ok = true;
277  if (route.valid() && src_route_position_)
278  {
279  sru::RoutePoint point;
280  if (sru::interpolateRoutePosition(point, route, *src_route_position_, true))
281  {
282  DrawRoutePoint(point);
283  }
284  else
285  {
286  PrintError("Failed to find route position in route.");
287  ok = false;
288  }
289  }
290 
291  if (ok)
292  {
293  PrintInfo("OK");
294  }
295  }
296 
297  void RoutePlugin::DrawStopWaypoint(double x, double y)
298  {
299  const double a = 2;
300  const double S = a * 2.414213562373095;
301 
302  glBegin(GL_POLYGON);
303 
304  glColor3f(1.0, 0.0, 0.0);
305 
306  glVertex2d(x + S / 2.0, y - a / 2.0);
307  glVertex2d(x + S / 2.0, y + a / 2.0);
308  glVertex2d(x + a / 2.0, y + S / 2.0);
309  glVertex2d(x - a / 2.0, y + S / 2.0);
310  glVertex2d(x - S / 2.0, y + a / 2.0);
311  glVertex2d(x - S / 2.0, y - a / 2.0);
312  glVertex2d(x - a / 2.0, y - S / 2.0);
313  glVertex2d(x + a / 2.0, y - S / 2.0);
314 
315  glEnd();
316  }
317 
319  {
320  const QColor color = ui_.color->color();
321  glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0);
322 
323  if (draw_style_ == LINES)
324  {
325  glLineWidth(3);
326  glBegin(GL_LINE_STRIP);
327  }
328  else
329  {
330  glPointSize(2);
331  glBegin(GL_POINTS);
332  }
333 
334  for (size_t i = 0; i < route.points.size(); i++)
335  {
336  glVertex2d(route.points[i].position().x(),
337  route.points[i].position().y());
338  }
339  glEnd();
340  }
341 
343  {
344  const double arrow_size = ui_.iconsize->value();
345 
346  tf::Vector3 v1(arrow_size, 0.0, 0.0);
347  tf::Vector3 v2(0.0, arrow_size / 2.0, 0.0);
348  tf::Vector3 v3(0.0, -arrow_size / 2.0, 0.0);
349 
350  tf::Transform point_g(point.orientation(), point.position());
351 
352  v1 = point_g * v1;
353  v2 = point_g * v2;
354  v3 = point_g * v3;
355 
356  const QColor color = ui_.positioncolor->color();
357  glLineWidth(3);
358  glBegin(GL_POLYGON);
359  glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0);
360  glVertex2d(v1.x(), v1.y());
361  glVertex2d(v2.x(), v2.y());
362  glVertex2d(v3.x(), v3.y());
363  glEnd();
364  }
365 
366  void RoutePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
367  {
368  if (node["topic"])
369  {
370  std::string route_topic;
371  node["topic"] >> route_topic;
372  ui_.topic->setText(route_topic.c_str());
373  }
374  if (node["color"])
375  {
376  std::string color;
377  node["color"] >> color;
378  ui_.color->setColor(QColor(color.c_str()));
379  }
380  if (node["postopic"])
381  {
382  std::string pos_topic;
383  node["postopic"] >> pos_topic;
384  ui_.positiontopic->setText(pos_topic.c_str());
385  }
386  if (node["poscolor"])
387  {
388  std::string poscolor;
389  node["poscolor"] >> poscolor;
390  ui_.positioncolor->setColor(QColor(poscolor.c_str()));
391  }
392 
393  if (node["draw_style"])
394  {
395  std::string draw_style;
396  node["draw_style"] >> draw_style;
397 
398  if (draw_style == "lines")
399  {
400  draw_style_ = LINES;
401  ui_.drawstyle->setCurrentIndex(0);
402  }
403  else if (draw_style == "points")
404  {
406  ui_.drawstyle->setCurrentIndex(1);
407  }
408  }
409 
410  TopicEdited();
412  }
413 
414  void RoutePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
415  {
416  std::string route_topic = ui_.topic->text().toStdString();
417  emitter << YAML::Key << "topic" << YAML::Value << route_topic;
418 
419  std::string color = ui_.color->color().name().toStdString();
420  emitter << YAML::Key << "color" << YAML::Value << color;
421 
422  std::string pos_topic = ui_.positiontopic->text().toStdString();
423  emitter << YAML::Key << "postopic" << YAML::Value << pos_topic;
424 
425  std::string pos_color = ui_.positioncolor->color().name().toStdString();
426  emitter << YAML::Key << "poscolor" << YAML::Value << pos_color;
427 
428  std::string draw_style = ui_.drawstyle->currentText().toStdString();
429  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
430  }
431 }
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
IconWidget * icon_
ros::NodeHandle node_
void PrintInfo(const std::string &message)
std_msgs::Header header
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
marti_nav_msgs::RoutePositionConstPtr src_route_position_
Definition: route_plugin.h:114
void LoadConfig(const YAML::Node &node, const std::string &path)
const tf::Vector3 & position() const
ros::Subscriber position_sub_
Definition: route_plugin.h:111
void Draw(double x, double y, double scale)
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)
bool Initialize(QGLWidget *canvas)
void PrintError(const std::string &message)
std::string target_frame_
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
swri_route_util::Route src_route_
Definition: route_plugin.h:113
void SetPixmap(QPixmap pixmap)
void DrawRoutePoint(const swri_route_util::RoutePoint &point)
void PrintWarning(const std::string &message)
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
void DrawStopWaypoint(double x, double y)
ros::Subscriber route_sub_
Definition: route_plugin.h:110
std::vector< RoutePoint > points
void PositionCallback(const marti_nav_msgs::RoutePositionConstPtr &msg)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
void RouteCallback(const marti_nav_msgs::RouteConstPtr &msg)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
const tf::Quaternion & orientation() const
void SetDrawStyle(QString style)
void DrawRoute(const swri_route_util::Route &route)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
QWidget * GetConfigWidget(QWidget *parent)


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