plan_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 <QDateTime>
38 #include <QDialog>
39 #include <QGLWidget>
40 #include <QMouseEvent>
41 #include <QPainter>
42 #include <QPalette>
43 #include <QStaticText>
44 
45 #include <opencv2/core/core.hpp>
46 
47 // ROS libraries
48 #include <ros/master.h>
49 
50 #include <swri_route_util/util.h>
52 
53 #include <marti_nav_msgs/PlanRoute.h>
54 
55 // Declare plugin
58 
59 namespace mnm = marti_nav_msgs;
60 namespace sru = swri_route_util;
61 namespace stu = swri_transform_util;
62 
63 namespace mapviz_plugins
64 {
66  config_widget_(new QWidget()),
67  map_canvas_(NULL),
68  failed_service_(false),
69  selected_point_(-1),
70  is_mouse_down_(false),
71  max_ms_(Q_INT64_C(500)),
72  max_distance_(2.0)
73  {
74  ui_.setupUi(config_widget_);
75 
76  ui_.color->setColor(Qt::green);
77  // Set background white
78  QPalette p(config_widget_->palette());
79  p.setColor(QPalette::Background, Qt::white);
80  config_widget_->setPalette(p);
81  // Set status text red
82  QPalette p3(ui_.status->palette());
83  p3.setColor(QPalette::Text, Qt::red);
84  ui_.status->setPalette(p3);
85  QObject::connect(ui_.service, SIGNAL(editingFinished()), this,
86  SLOT(PlanRoute()));
87  QObject::connect(ui_.publish, SIGNAL(clicked()), this,
88  SLOT(PublishRoute()));
89  QObject::connect(ui_.clear, SIGNAL(clicked()), this,
90  SLOT(Clear()));
91  QObject::connect(this,
92  SIGNAL(VisibleChanged(bool)),
93  this,
94  SLOT(VisibilityChanged(bool)));
95  }
96 
98  {
99  if (map_canvas_)
100  {
101  map_canvas_->removeEventFilter(this);
102  }
103  }
104 
106  {
107  if (visible)
108  {
109  map_canvas_->installEventFilter(this);
110  }
111  else
112  {
113  map_canvas_->removeEventFilter(this);
114  }
115  }
116 
118  {
119  if (route_preview_)
120  {
121  if (route_topic_ != ui_.topic->text().toStdString())
122  {
123  route_topic_ = ui_.topic->text().toStdString();
126  }
127 
129  }
130  }
131 
133  {
135  bool start_from_vehicle = ui_.start_from_vehicle->isChecked();
136  if (waypoints_.size() + start_from_vehicle < 2 || !Visible())
137  {
138  return;
139  }
140 
141  std::string service = ui_.service->text().toStdString();
142  ros::ServiceClient client = node_.serviceClient<mnm::PlanRoute>(service);
143 
144  mnm::PlanRoute plan_route;
145  plan_route.request.header.frame_id = stu::_wgs84_frame;
146  plan_route.request.header.stamp = ros::Time::now();
147  plan_route.request.plan_from_vehicle = static_cast<unsigned char>(start_from_vehicle);
148  plan_route.request.waypoints = waypoints_;
149 
150  if (client.call(plan_route))
151  {
152  if (plan_route.response.success)
153  {
154  route_preview_ = boost::make_shared<sru::Route>(plan_route.response.route);
155  failed_service_ = false;
156  }
157  else
158  {
159  PrintError(plan_route.response.message);
160  failed_service_ = true;
161  }
162  }
163  else
164  {
165  PrintError("Failed to plan route.");
166  failed_service_ = true;
167  }
168  }
169 
171  {
172  PlanRoute();
173  }
174 
176  {
177  waypoints_.clear();
179  }
180 
181  void PlanRoutePlugin::PrintError(const std::string& message)
182  {
183  PrintErrorHelper(ui_.status, message, 1.0);
184  }
185 
186  void PlanRoutePlugin::PrintInfo(const std::string& message)
187  {
188  PrintInfoHelper(ui_.status, message, 1.0);
189  }
190 
191  void PlanRoutePlugin::PrintWarning(const std::string& message)
192  {
193  PrintWarningHelper(ui_.status, message, 1.0);
194  }
195 
196  QWidget* PlanRoutePlugin::GetConfigWidget(QWidget* parent)
197  {
198  config_widget_->setParent(parent);
199 
200  return config_widget_;
201  }
202 
203  bool PlanRoutePlugin::Initialize(QGLWidget* canvas)
204  {
205  map_canvas_ = static_cast<mapviz::MapCanvas*>(canvas);
206  map_canvas_->installEventFilter(this);
207 
209 
210  initialized_ = true;
211  return true;
212  }
213 
214  bool PlanRoutePlugin::eventFilter(QObject *object, QEvent* event)
215  {
216  switch (event->type())
217  {
218  case QEvent::MouseButtonPress:
219  return handleMousePress(static_cast<QMouseEvent*>(event));
220  case QEvent::MouseButtonRelease:
221  return handleMouseRelease(static_cast<QMouseEvent*>(event));
222  case QEvent::MouseMove:
223  return handleMouseMove(static_cast<QMouseEvent*>(event));
224  default:
225  return false;
226  }
227  }
228 
229  bool PlanRoutePlugin::handleMousePress(QMouseEvent* event)
230  {
231  selected_point_ = -1;
232  int closest_point = 0;
233  double closest_distance = std::numeric_limits<double>::max();
234 
235 #if QT_VERSION >= 0x050000
236  QPointF point = event->localPos();
237 #else
238  QPointF point = event->posF();
239 #endif
241  if (tf_manager_->GetTransform(target_frame_, stu::_wgs84_frame, transform))
242  {
243  for (size_t i = 0; i < waypoints_.size(); i++)
244  {
245  tf::Vector3 waypoint(
246  waypoints_[i].position.x,
247  waypoints_[i].position.y,
248  0.0);
249  waypoint = transform * waypoint;
250 
251  QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(QPointF(waypoint.x(), waypoint.y()));
252 
253  double distance = QLineF(transformed, point).length();
254 
255  if (distance < closest_distance)
256  {
257  closest_distance = distance;
258  closest_point = static_cast<int>(i);
259  }
260  }
261  }
262 
263  if (event->button() == Qt::LeftButton)
264  {
265  if (closest_distance < 15)
266  {
267  selected_point_ = closest_point;
268  return true;
269  }
270  else
271  {
272  is_mouse_down_ = true;
273 #if QT_VERSION >= 0x050000
274  mouse_down_pos_ = event->localPos();
275 #else
276  mouse_down_pos_ = event->posF();
277 #endif
278  mouse_down_time_ = QDateTime::currentMSecsSinceEpoch();
279  return false;
280  }
281  }
282  else if (event->button() == Qt::RightButton)
283  {
284  if (closest_distance < 15)
285  {
286  waypoints_.erase(waypoints_.begin() + closest_point);
287  PlanRoute();
288  return true;
289  }
290  }
291 
292  return false;
293  }
294 
295  bool PlanRoutePlugin::handleMouseRelease(QMouseEvent* event)
296  {
297 #if QT_VERSION >= 0x050000
298  QPointF point = event->localPos();
299 #else
300  QPointF point = event->posF();
301 #endif
302  if (selected_point_ >= 0 && static_cast<size_t>(selected_point_) < waypoints_.size())
303  {
305  if (tf_manager_->GetTransform(stu::_wgs84_frame, target_frame_, transform))
306  {
307  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
308  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
309  position = transform * position;
310  waypoints_[selected_point_].position.x = position.x();
311  waypoints_[selected_point_].position.y = position.y();
312  PlanRoute();
313  }
314 
315  selected_point_ = -1;
316  return true;
317  }
318  else if (is_mouse_down_)
319  {
320  qreal distance = QLineF(mouse_down_pos_, point).length();
321  qint64 msecsDiff = QDateTime::currentMSecsSinceEpoch() - mouse_down_time_;
322 
323  // Only fire the event if the mouse has moved less than the maximum distance
324  // and was held for shorter than the maximum time.. This prevents click
325  // events from being fired if the user is dragging the mouse across the map
326  // or just holding the cursor in place.
327  if (msecsDiff < max_ms_ && distance <= max_distance_)
328  {
329  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
330 
332  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
333  if (tf_manager_->GetTransform(stu::_wgs84_frame, target_frame_, transform))
334  {
335  position = transform * position;
336 
337  geometry_msgs::Pose pose;
338  pose.position.x = position.x();
339  pose.position.y = position.y();
340  waypoints_.push_back(pose);
341  PlanRoute();
342  }
343  }
344  }
345  is_mouse_down_ = false;
346 
347  return false;
348  }
349 
350  bool PlanRoutePlugin::handleMouseMove(QMouseEvent* event)
351  {
352  if (selected_point_ >= 0 && static_cast<size_t>(selected_point_) < waypoints_.size())
353  {
354 #if QT_VERSION >= 0x050000
355  QPointF point = event->localPos();
356 #else
357  QPointF point = event->posF();
358 #endif
360  if (tf_manager_->GetTransform(stu::_wgs84_frame, target_frame_, transform))
361  {
362  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
363  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
364  position = transform * position;
365  waypoints_[selected_point_].position.y = position.y();
366  waypoints_[selected_point_].position.x = position.x();
367  PlanRoute();
368  }
369 
370  return true;
371  }
372  return false;
373  }
374 
375  void PlanRoutePlugin::Draw(double x, double y, double scale)
376  {
378  if (tf_manager_->GetTransform(target_frame_, stu::_wgs84_frame, transform))
379  {
380  if (!failed_service_)
381  {
382  if (route_preview_)
383  {
384  sru::Route route = *route_preview_;
385  sru::transform(route, transform, target_frame_);
386 
387  glLineWidth(2);
388  const QColor color = ui_.color->color();
389  glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0);
390  glBegin(GL_LINE_STRIP);
391 
392  for (size_t i = 0; i < route.points.size(); i++)
393  {
394  glVertex2d(route.points[i].position().x(), route.points[i].position().y());
395  }
396 
397  glEnd();
398  }
399 
400  PrintInfo("OK");
401  }
402 
403  // Draw waypoints
404 
405  glPointSize(20);
406  glColor4f(0.0, 1.0, 1.0, 1.0);
407  glBegin(GL_POINTS);
408 
409  for (size_t i = 0; i < waypoints_.size(); i++)
410  {
411  tf::Vector3 point(waypoints_[i].position.x, waypoints_[i].position.y, 0);
412  point = transform * point;
413  glVertex2d(point.x(), point.y());
414  }
415  glEnd();
416  }
417  else
418  {
419  PrintError("Failed to transform.");
420  }
421  }
422 
423  void PlanRoutePlugin::Paint(QPainter* painter, double x, double y, double scale)
424  {
425  painter->save();
426  painter->resetTransform();
427 
428  QPen pen(QBrush(QColor(Qt::darkCyan).darker()), 1);
429  painter->setPen(pen);
430  painter->setFont(QFont("DejaVu Sans Mono", 7));
431 
433  if (tf_manager_->GetTransform(target_frame_, stu::_wgs84_frame, transform))
434  {
435  for (size_t i = 0; i < waypoints_.size(); i++)
436  {
437  tf::Vector3 point(waypoints_[i].position.x, waypoints_[i].position.y, 0);
438  point = transform * point;
439  QPointF gl_point = map_canvas_->FixedFrameToMapGlCoord(QPointF(point.x(), point.y()));
440  QPointF corner(gl_point.x() - 20, gl_point.y() - 20);
441  QRectF rect(corner, QSizeF(40, 40));
442  painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::fromStdString(boost::lexical_cast<std::string>(i + 1)));
443  }
444  }
445 
446  painter->restore();
447  }
448 
449  void PlanRoutePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
450  {
451  if (node["route_topic"])
452  {
453  std::string route_topic;
454  node["route_topic"] >> route_topic;
455  ui_.topic->setText(route_topic.c_str());
456  }
457  if (node["color"])
458  {
459  std::string color;
460  node["color"] >> color;
461  ui_.color->setColor(QColor(color.c_str()));
462  }
463  if (node["service"])
464  {
465  std::string service;
466  node["service"] >> service;
467  ui_.service->setText(service.c_str());
468  }
469  if (node["start_from_vehicle"])
470  {
471  bool start_from_vehicle;
472  node["start_from_vehicle"] >> start_from_vehicle;
473  ui_.start_from_vehicle->setChecked(start_from_vehicle);
474  }
475 
476  PlanRoute();
477  }
478 
479  void PlanRoutePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
480  {
481  std::string route_topic = ui_.topic->text().toStdString();
482  emitter << YAML::Key << "route_topic" << YAML::Value << route_topic;
483 
484  std::string color = ui_.color->color().name().toStdString();
485  emitter << YAML::Key << "color" << YAML::Value << color;
486 
487  std::string service = ui_.service->text().toStdString();
488  emitter << YAML::Key << "service" << YAML::Value << service;
489 
490  bool start_from_vehicle = ui_.start_from_vehicle->isChecked();
491  emitter << YAML::Key << "start_from_vehicle" << YAML::Value << start_from_vehicle;
492  }
493 }
#define NULL
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void Retry(const ros::TimerEvent &e)
QPointF MapGlCoordToFixedFrame(const QPointF &point)
std::vector< geometry_msgs::Pose > waypoints_
ros::NodeHandle node_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void Paint(QPainter *painter, double x, double y, double scale)
bool call(MReq &req, MRes &res)
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)
swri_route_util::RoutePtr route_preview_
std::string target_frame_
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void LoadConfig(const YAML::Node &node, const std::string &path)
void PrintError(const std::string &message)
void PrintInfo(const std::string &message)
void Draw(double x, double y, double scale)
bool Visible() const
bool eventFilter(QObject *object, QEvent *event)
void publish(const boost::shared_ptr< M > &message) const
QPointF FixedFrameToMapGlCoord(const QPointF &point)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< RoutePoint > points
bool Initialize(QGLWidget *canvas)
void VisibleChanged(bool visible)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
void PrintWarning(const std::string &message)
static Time now()
swri_transform_util::TransformManagerPtr tf_manager_
#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