grid_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 #include <boost/lexical_cast.hpp>
37 
38 // QT libraries
39 #include <QGLWidget>
40 #include <QPalette>
41 
43 
44 // Declare plugin
47 
48 namespace mapviz_plugins
49 {
51  config_widget_(new QWidget()),
52  alpha_(1.0),
53  top_left_(0, 0, 0),
54  size_(1),
55  rows_(1),
56  columns_(1),
57  transformed_(false)
58  {
59  ui_.setupUi(config_widget_);
60 
61  ui_.color->setColor(Qt::red);
62 
63  // Set background white
64  QPalette p(config_widget_->palette());
65  p.setColor(QPalette::Background, Qt::white);
66  config_widget_->setPalette(p);
67 
68  // Set status text red
69  QPalette p3(ui_.status->palette());
70  p3.setColor(QPalette::Text, Qt::red);
71  ui_.status->setPalette(p3);
72 
73  QObject::connect(ui_.select_frame, SIGNAL(clicked()), this, SLOT(SelectFrame()));
74  QObject::connect(ui_.frame, SIGNAL(textEdited(const QString&)), this, SLOT(FrameEdited()));
75  QObject::connect(ui_.alpha, SIGNAL(valueChanged(double)), this, SLOT(SetAlpha(double)));
76  QObject::connect(ui_.x, SIGNAL(valueChanged(double)), this, SLOT(SetX(double)));
77  QObject::connect(ui_.y, SIGNAL(valueChanged(double)), this, SLOT(SetY(double)));
78  QObject::connect(ui_.size, SIGNAL(valueChanged(double)), this, SLOT(SetSize(double)));
79  QObject::connect(ui_.rows, SIGNAL(valueChanged(int)), this, SLOT(SetRows(int)));
80  QObject::connect(ui_.columns, SIGNAL(valueChanged(int)), this, SLOT(SetColumns(int)));
81  connect(ui_.color, SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon()));
82  }
83 
85  {
86  Shutdown();
87  }
88 
90  {
91  }
92 
94  {
95  if (icon_)
96  {
97  QPixmap icon(16, 16);
98  icon.fill(Qt::transparent);
99 
100  QPainter painter(&icon);
101  painter.setRenderHint(QPainter::Antialiasing, true);
102 
103  QPen pen(QColor(ui_.color->color()));
104 
105  pen.setWidth(2);
106  pen.setCapStyle(Qt::SquareCap);
107  painter.setPen(pen);
108 
109  painter.drawLine(2, 2, 14, 2);
110  painter.drawLine(2, 2, 2, 14);
111  painter.drawLine(14, 2, 14, 14);
112  painter.drawLine(2, 14, 14, 14);
113  painter.drawLine(8, 2, 8, 14);
114  painter.drawLine(2, 8, 14, 8);
115 
116  icon_->SetPixmap(icon);
117  }
118  }
119 
120  void GridPlugin::SetAlpha(double alpha)
121  {
122  alpha_ = alpha;
123  }
124 
125  void GridPlugin::SetX(double x)
126  {
127  top_left_.setX(x);
128 
129  RecalculateGrid();
130  }
131 
132  void GridPlugin::SetY(double y)
133  {
134  top_left_.setY(y);
135 
136  RecalculateGrid();
137  }
138 
139  void GridPlugin::SetSize(double size)
140  {
141  size_ = size;
142 
143  RecalculateGrid();
144  }
145 
146  void GridPlugin::SetRows(int rows)
147  {
148  rows_ = rows;
149 
150  RecalculateGrid();
151  }
152 
153  void GridPlugin::SetColumns(int columns)
154  {
155  columns_ = columns;
156 
157  RecalculateGrid();
158  }
159 
161  {
162  std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_);
163  if (!frame.empty())
164  {
165  ui_.frame->setText(QString::fromStdString(frame));
166  FrameEdited();
167  }
168  }
169 
171  {
172  source_frame_ = ui_.frame->text().toStdString();
173 
174  initialized_ = true;
175 
176  RecalculateGrid();
177  }
178 
179  void GridPlugin::PrintError(const std::string& message)
180  {
181  PrintErrorHelper(ui_.status, message);
182  }
183 
184  void GridPlugin::PrintInfo(const std::string& message)
185  {
186  PrintInfoHelper(ui_.status, message);
187  }
188 
189  void GridPlugin::PrintWarning(const std::string& message)
190  {
191  PrintWarningHelper(ui_.status, message);
192  }
193 
194  QWidget* GridPlugin::GetConfigWidget(QWidget* parent)
195  {
196  config_widget_->setParent(parent);
197 
198  return config_widget_;
199  }
200 
201  bool GridPlugin::Initialize(QGLWidget* canvas)
202  {
203  canvas_ = canvas;
204 
205  DrawIcon();
206 
207  return true;
208  }
209 
210  void GridPlugin::Draw(double x, double y, double scale)
211  {
212  if (transformed_)
213  {
214  QColor color = ui_.color->color();
215 
216  glLineWidth(3);
217  glColor4d(color.redF(), color.greenF(), color.blueF(), alpha_);
218  glBegin(GL_LINES);
219 
220  std::list<tf::Point>::iterator transformed_left_it = transformed_left_points_.begin();
221  std::list<tf::Point>::iterator transformed_right_it = transformed_right_points_.begin();
222  for (; transformed_left_it != transformed_left_points_.end(); ++transformed_left_it)
223  {
224  glVertex2d(transformed_left_it->getX(), transformed_left_it->getY());
225  glVertex2d(transformed_right_it->getX(), transformed_right_it->getY());
226 
227  ++transformed_right_it;
228  }
229 
230  std::list<tf::Point>::iterator transformed_top_it = transformed_top_points_.begin();
231  std::list<tf::Point>::iterator transformed_bottom_it = transformed_bottom_points_.begin();
232  for (; transformed_top_it != transformed_top_points_.end(); ++transformed_top_it)
233  {
234  glVertex2d(transformed_top_it->getX(), transformed_top_it->getY());
235  glVertex2d(transformed_bottom_it->getX(), transformed_bottom_it->getY());
236 
237  ++transformed_bottom_it;
238  }
239 
240  glEnd();
241 
242  PrintInfo("OK");
243  }
244  }
245 
247  {
248  transformed_ = false;
249 
250  left_points_.clear();
251  right_points_.clear();
252  top_points_.clear();
253  bottom_points_.clear();
254 
255  transformed_left_points_.clear();
257  transformed_top_points_.clear();
259 
260  // Set top and bottom
261  for (int c = 0; c <= columns_; c++)
262  {
263  tf::Point top_point(top_left_.getX() + c * size_, top_left_.getY(), 0);
264  top_points_.push_back(top_point);
265  transformed_top_points_.push_back(transform_ * top_point);
266 
267  tf::Point bottom_point(top_left_.getX() + c * size_, top_left_.getY() + size_ * rows_, 0);
268  bottom_points_.push_back(bottom_point);
269  transformed_bottom_points_.push_back(transform_ * bottom_point);
270  }
271 
272  // Set left and right
273  for (int r = 0; r <= rows_; r++)
274  {
275  tf::Point left_point(top_left_.getX(), top_left_.getY() + r * size_, 0);
276  left_points_.push_back(left_point);
277  transformed_left_points_.push_back(transform_ * left_point);
278 
279  tf::Point right_point(top_left_.getX() + size_ * columns_, top_left_.getY() + r * size_, 0);
280  right_points_.push_back(right_point);
281  transformed_right_points_.push_back(transform_ * right_point);
282  }
283  }
284 
286  {
287  transformed_ = false;
288 
290  {
295 
296  transformed_ = true;
297  }
298  }
299 
300  void GridPlugin::Transform(std::list<tf::Point>& src, std::list<tf::Point>& dst)
301  {
302  std::list<tf::Point>::iterator points_it = src.begin();
303  std::list<tf::Point>::iterator transformed_it = dst.begin();
304  for (; points_it != src.end() && transformed_it != dst.end(); ++points_it)
305  {
306  (*transformed_it) = transform_ * (*points_it);
307 
308  ++transformed_it;
309  }
310  }
311 
312  void GridPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
313  {
314  if (node["color"])
315  {
316  std::string color;
317  node["color"] >> color;
318  ui_.color->setColor(QColor(color.c_str()));
319  }
320 
321  if (node["frame"])
322  {
323  std::string frame;
324  node["frame"] >> frame;
325  ui_.frame->setText(QString::fromStdString(frame));
326  }
327 
328  if (node["x"])
329  {
330  float x = 0;
331  node["x"] >> x;
332  ui_.x->setValue(x);
333  }
334 
335  if (node["y"])
336  {
337  float y = 0;
338  node["y"] >> y;
339  ui_.y->setValue(y);
340  }
341 
342  if (node["alpha"])
343  {
344  node["alpha"] >> alpha_;
345  ui_.alpha->setValue(alpha_);
346  }
347 
348  if (node["size"])
349  {
350  node["size"] >> size_;
351  ui_.size->setValue(size_);
352  }
353 
354  if (node["rows"])
355  {
356  node["rows"] >> rows_;
357  ui_.rows->setValue(rows_);
358  }
359 
360  if (node["columns"])
361  {
362  node["columns"] >> columns_;
363  ui_.columns->setValue(columns_);
364  }
365 
366  FrameEdited();
367  }
368 
369  void GridPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
370  {
371  emitter << YAML::Key << "color" << YAML::Value << ui_.color->color().name().toStdString();
372 
373  emitter << YAML::Key << "alpha" << YAML::Value << alpha_;
374 
375  std::string frame = ui_.frame->text().toStdString();
376  emitter << YAML::Key << "frame" << YAML::Value << frame;
377 
378  emitter << YAML::Key << "x" << YAML::Value << top_left_.getX();
379  emitter << YAML::Key << "y" << YAML::Value << top_left_.getY();
380  emitter << YAML::Key << "size" << YAML::Value << size_;
381  emitter << YAML::Key << "rows" << YAML::Value << rows_;
382  emitter << YAML::Key << "columns" << YAML::Value << columns_;
383  }
384 }
385 
mapviz_plugins::GridPlugin::Initialize
bool Initialize(QGLWidget *canvas)
Definition: grid_plugin.cpp:201
mapviz::MapvizPlugin::tf_
boost::shared_ptr< tf::TransformListener > tf_
mapviz_plugins::GridPlugin::ui_
Ui::grid_config ui_
Definition: grid_plugin.h:93
mapviz_plugins::GridPlugin::rows_
int rows_
Definition: grid_plugin.h:101
mapviz_plugins::GridPlugin::right_points_
std::list< tf::Point > right_points_
Definition: grid_plugin.h:109
mapviz::MapvizPlugin::PrintErrorHelper
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz::SelectFrameDialog::selectFrame
static std::string selectFrame(boost::shared_ptr< tf::TransformListener > tf_listener, QWidget *parent=0)
mapviz_plugins::GridPlugin::transformed_
bool transformed_
Definition: grid_plugin.h:104
mapviz_plugins::GridPlugin::DrawIcon
void DrawIcon()
Definition: grid_plugin.cpp:93
mapviz_plugins::GridPlugin::RecalculateGrid
void RecalculateGrid()
Definition: grid_plugin.cpp:246
mapviz_plugins::GridPlugin::GridPlugin
GridPlugin()
Definition: grid_plugin.cpp:50
mapviz_plugins::GridPlugin::config_widget_
QWidget * config_widget_
Definition: grid_plugin.h:94
mapviz_plugins::GridPlugin::PrintWarning
void PrintWarning(const std::string &message)
Definition: grid_plugin.cpp:189
mapviz_plugins::GridPlugin::SetRows
void SetRows(int rows)
Definition: grid_plugin.cpp:146
class_list_macros.h
mapviz_plugins::GridPlugin::SetColumns
void SetColumns(int columns)
Definition: grid_plugin.cpp:153
mapviz_plugins::GridPlugin::FrameEdited
void FrameEdited()
Definition: grid_plugin.cpp:170
mapviz_plugins::GridPlugin::SetY
void SetY(double y)
Definition: grid_plugin.cpp:132
tf::Point
tf::Vector3 Point
mapviz_plugins::GridPlugin::SetSize
void SetSize(double size)
Definition: grid_plugin.cpp:139
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
mapviz_plugins::GridPlugin::GetConfigWidget
QWidget * GetConfigWidget(QWidget *parent)
Definition: grid_plugin.cpp:194
mapviz_plugins::GridPlugin::LoadConfig
void LoadConfig(const YAML::Node &node, const std::string &path)
Definition: grid_plugin.cpp:312
mapviz_plugins::GridPlugin::PrintInfo
void PrintInfo(const std::string &message)
Definition: grid_plugin.cpp:184
mapviz_plugins::GridPlugin::alpha_
double alpha_
Definition: grid_plugin.h:96
mapviz::MapvizPlugin::PrintWarningHelper
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz_plugins::GridPlugin::SaveConfig
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
Definition: grid_plugin.cpp:369
mapviz_plugins::GridPlugin::transformed_right_points_
std::list< tf::Point > transformed_right_points_
Definition: grid_plugin.h:114
mapviz_plugins::GridPlugin::Transform
void Transform()
Definition: grid_plugin.cpp:285
mapviz_plugins::GridPlugin::left_points_
std::list< tf::Point > left_points_
Definition: grid_plugin.h:108
mapviz_plugins::GridPlugin::columns_
int columns_
Definition: grid_plugin.h:102
mapviz::MapvizPlugin::PrintInfoHelper
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz_plugins::GridPlugin::~GridPlugin
virtual ~GridPlugin()
Definition: grid_plugin.cpp:84
mapviz::MapvizPlugin::canvas_
QGLWidget * canvas_
mapviz_plugins::GridPlugin::transform_
swri_transform_util::Transform transform_
Definition: grid_plugin.h:116
ros::Time
mapviz_plugins::GridPlugin::top_left_
tf::Point top_left_
Definition: grid_plugin.h:98
mapviz_plugins::GridPlugin::Shutdown
void Shutdown()
Definition: grid_plugin.cpp:89
mapviz_plugins::GridPlugin::transformed_top_points_
std::list< tf::Point > transformed_top_points_
Definition: grid_plugin.h:111
grid_plugin.h
mapviz_plugins::GridPlugin::SelectFrame
void SelectFrame()
Definition: grid_plugin.cpp:160
mapviz_plugins::GridPlugin::bottom_points_
std::list< tf::Point > bottom_points_
Definition: grid_plugin.h:107
select_frame_dialog.h
mapviz_plugins::GridPlugin::SetX
void SetX(double x)
Definition: grid_plugin.cpp:125
mapviz::MapvizPlugin
mapviz::MapvizPlugin::initialized_
bool initialized_
mapviz_plugins::GridPlugin::transformed_bottom_points_
std::list< tf::Point > transformed_bottom_points_
Definition: grid_plugin.h:112
mapviz::MapvizPlugin::source_frame_
std::string source_frame_
mapviz_plugins::GridPlugin::size_
double size_
Definition: grid_plugin.h:100
mapviz_plugins::GridPlugin
Definition: grid_plugin.h:56
mapviz_plugins::GridPlugin::top_points_
std::list< tf::Point > top_points_
Definition: grid_plugin.h:106
mapviz_plugins::GridPlugin::Draw
void Draw(double x, double y, double scale)
Definition: grid_plugin.cpp:210
mapviz::IconWidget::SetPixmap
void SetPixmap(QPixmap pixmap)
mapviz::MapvizPlugin::icon_
IconWidget * icon_
mapviz_plugins
Definition: attitude_indicator_plugin.h:61
mapviz_plugins::GridPlugin::SetAlpha
void SetAlpha(double alpha)
Definition: grid_plugin.cpp:120
mapviz::MapvizPlugin::GetTransform
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform)
mapviz_plugins::GridPlugin::transformed_left_points_
std::list< tf::Point > transformed_left_points_
Definition: grid_plugin.h:113
mapviz_plugins::GridPlugin::PrintError
void PrintError(const std::string &message)
Definition: grid_plugin.cpp:179


mapviz_plugins
Author(s): Marc Alban
autogenerated on Sun Sep 8 2024 02:27:14