coordinate_picker_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2018, 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 #include <mapviz/mapviz_plugin.h>
32 
33 #include <QClipboard>
34 #include <QMouseEvent>
35 #include <QTextStream>
36 
37 #if QT_VERSION >= 0x050000
38 #include <QGuiApplication>
39 #else
40 #include <QApplication>
41 #endif
42 
43 // ROS Libraries
44 #include <ros/ros.h>
45 
46 // Mapviz Libraries
48 
49 //
53 
56 
57 namespace mapviz_plugins
58 {
59 
61  : config_widget_(new QWidget()),
62  map_canvas_(NULL),
63  copy_on_click_(false)
64 {
65  ui_.setupUi(config_widget_);
66 
67  QObject::connect(ui_.selectframe, SIGNAL(clicked()),
68  this, SLOT(SelectFrame()));
69  QObject::connect(ui_.frame, SIGNAL(editingFinished()),
70  this, SLOT(FrameEdited()));
71  QObject::connect(ui_.copyCheckBox, SIGNAL(stateChanged(int)),
72  this, SLOT(ToggleCopyOnClick(int)));
73  QObject::connect(ui_.clearListButton, SIGNAL(clicked()),
74  this, SLOT(ClearCoordList()));
75 
76 #if QT_VERSION >= 0x050000
77  ui_.coordTextEdit->setPlaceholderText(tr("Click on the map; coordinates appear here"));
78 #endif
79 }
80 
82 {
83  if (map_canvas_)
84  {
85  map_canvas_->removeEventFilter(this);
86  }
87 }
88 
89 QWidget* CoordinatePickerPlugin::GetConfigWidget(QWidget* parent)
90 {
91  config_widget_->setParent(parent);
92 
93  return config_widget_;
94 }
95 
96 bool CoordinatePickerPlugin::Initialize(QGLWidget* canvas)
97 {
98  map_canvas_ = static_cast< mapviz::MapCanvas* >(canvas);
99  map_canvas_->installEventFilter(this);
100 
101  initialized_ = true;
102  PrintInfo("OK");
103 
104  return true;
105 }
106 
107 bool CoordinatePickerPlugin::eventFilter(QObject* object, QEvent* event)
108 {
109  if(!this->Visible())
110  {
111  ROS_DEBUG("Ignoring mouse event, since coordinate picker plugin is hidden");
112  return false;
113  }
114 
115  switch (event->type())
116  {
117  case QEvent::MouseButtonPress:
118  return handleMousePress(static_cast< QMouseEvent* >(event));
119  case QEvent::MouseButtonRelease:
120  return handleMouseRelease(static_cast< QMouseEvent* >(event));
121  case QEvent::MouseMove:
122  return handleMouseMove(static_cast< QMouseEvent* >(event));
123  default:
124  return false;
125  }
126 }
127 
129 {
130 #if QT_VERSION >= 0x050000
131  QPointF point = event->localPos();
132 #else
133  QPointF point = event->posF();
134 #endif
135  ROS_DEBUG("Map point: %f %f", point.x(), point.y());
136 
138  std::string frame = ui_.frame->text().toStdString();
139  if (frame.empty())
140  {
141  frame = target_frame_;
142  }
143 
144  // Frames get confusing. The `target_frame_` member is set by the "Fixed
145  // Frame" combobox in the GUI. When we transform the map coordinate to the
146  // fixed frame, we get it in the `target_frame_` frame.
147  //
148  // Then we translate from that frame into *our* target frame, `frame`.
149  if (tf_manager_->GetTransform(frame, target_frame_, transform))
150  {
151  ROS_DEBUG("Transforming from fixed frame '%s' to (plugin) target frame '%s'",
152  target_frame_.c_str(),
153  frame.c_str());
154  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
155  ROS_DEBUG("Point in fixed frame: %f %f", transformed.x(), transformed.y());
156  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
157  position = transform * position;
158  point.setX(position.x());
159  point.setY(position.y());
160 
161  PrintInfo("OK");
162  }
163  else
164  {
165  QString warning;
166  QTextStream(&warning) << "No available transform from '" << QString::fromStdString(target_frame_) << "' to '" << QString::fromStdString(frame) << "'";
167  PrintWarning(warning.toStdString());
168  return false;
169  }
170 
171 
172  ROS_DEBUG("Transformed point in frame '%s': %f %f", frame.c_str(), point.x(), point.y());
173  QString new_point;
174  QTextStream stream(&new_point);
176  {
177  stream.setRealNumberPrecision(9);
178  }
179  else
180  {
181  stream.setRealNumberPrecision(4);
182  }
183  stream << point.x() << ", " << point.y();
184 
185  if (copy_on_click_)
186  {
187 #if QT_VERSION >= 0x050000
188  QClipboard* clipboard = QGuiApplication::clipboard();
189 #else
190  QClipboard* clipboard = QApplication::clipboard();
191 #endif
192  clipboard->setText(new_point);
193  }
194 
195  stream << " (" << QString::fromStdString(frame) << ")\n";
196 
197  ui_.coordTextEdit->setPlainText(ui_.coordTextEdit->toPlainText().prepend(new_point));
198 
199  // Let other plugins process this event too
200  return false;
201 }
202 
204 {
205  // Let other plugins process this event too
206  return false;
207 }
208 
210 {
211  // Let other plugins process this event too
212  return false;
213 }
214 
216 {
217  std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_);
218  if (!frame.empty())
219  {
220  ui_.frame->setText(QString::fromStdString(frame));
221  FrameEdited();
222  }
223 }
224 
226 {
227  ROS_INFO("Setting target frame to %s", ui_.frame->text().toStdString().c_str());
228 }
229 
231 {
232  switch(state)
233  {
234  case Qt::Checked:
235  copy_on_click_ = true;
236  break;
237  case Qt::PartiallyChecked:
238  case Qt::Unchecked:
239  default:
240  copy_on_click_ = false;
241  break;
242  }
243 }
244 
246 {
247  ui_.coordTextEdit->setPlainText(QString());
248 }
249 
250 void CoordinatePickerPlugin::Draw(double x, double y, double scale)
251 {
252 }
253 
254 void CoordinatePickerPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
255 {
256  if (node["frame"])
257  {
258  std::string frame;
259  node["frame"] >> frame;
260  ui_.frame->setText(QString::fromStdString(frame));
261  }
262 
263  if (node["copy"])
264  {
265  bool copy;
266  node["copy"] >> copy;
267  if (copy)
268  {
269  ui_.copyCheckBox->setCheckState(Qt::Checked);
270  }
271  else
272  {
273  ui_.copyCheckBox->setCheckState(Qt::Unchecked);
274  }
275  }
276 }
277 
278 void CoordinatePickerPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
279 {
280  std::string frame = ui_.frame->text().toStdString();
281  emitter << YAML::Key << "frame" << YAML::Value << frame;
282 
283  bool copy_on_click = ui_.copyCheckBox->isChecked();
284  emitter << YAML::Key << "copy" << YAML::Value << copy_on_click;
285 }
286 
287 void CoordinatePickerPlugin::PrintError(const std::string& message)
288 {
289  PrintErrorHelper(ui_.status, message, 1.0);
290 }
291 
292 void CoordinatePickerPlugin::PrintInfo(const std::string& message)
293 {
294  PrintInfoHelper(ui_.status, message, 1.0);
295 }
296 
297 void CoordinatePickerPlugin::PrintWarning(const std::string& message)
298 {
299  PrintWarningHelper(ui_.status, message, 1.0);
300 }
301 
302 } // namespace mapviz_plugins
#define NULL
QPointF MapGlCoordToFixedFrame(const QPointF &point)
static const std::string _wgs84_frame
void PrintInfo(const std::string &message)
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)
std::string target_frame_
static std::string selectFrame(boost::shared_ptr< tf::TransformListener > tf_listener, QWidget *parent=0)
bool FrameIdsEqual(const std::string &frame1, const std::string &frame2)
bool eventFilter(QObject *object, QEvent *event)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
bool Visible() const
void PrintError(const std::string &message)
void PrintWarning(const std::string &message)
#define ROS_INFO(...)
void Draw(double x, double y, double scale)
void LoadConfig(const YAML::Node &node, const std::string &path)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
boost::shared_ptr< tf::TransformListener > tf_
swri_transform_util::TransformManagerPtr tf_manager_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_DEBUG(...)


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