odometry_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 
50 
51 // Declare plugin
54 
55 namespace mapviz_plugins
56 {
57  OdometryPlugin::OdometryPlugin() : config_widget_(new QWidget())
58  {
59  ui_.setupUi(config_widget_);
60  ui_.color->setColor(Qt::green);
61 
62  // Set background white
63  QPalette p(config_widget_->palette());
64  p.setColor(QPalette::Background, Qt::white);
65  config_widget_->setPalette(p);
66 
67  // Set status text red
68  QPalette p3(ui_.status->palette());
69  p3.setColor(QPalette::Text, Qt::red);
70  ui_.status->setPalette(p3);
71 
72  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
73  SLOT(SelectTopic()));
74  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
75  SLOT(TopicEdited()));
76  QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
77  SLOT(PositionToleranceChanged(double)));
78  QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this,
79  SLOT(BufferSizeChanged(int)));
80  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
81  SLOT(SetDrawStyle(QString)));
82  QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)),
83  this, SLOT(SetStaticArrowSizes(bool)));
84  QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
85  this, SLOT(SetArrowSize(int)));
86  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
87  SLOT(SetColor(const QColor&)));
88  QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this,
89  SLOT(LapToggled(bool)));
90  QObject::connect(ui_.show_covariance, SIGNAL(toggled(bool)), this,
91  SLOT(CovariancedToggled(bool)));
92  QObject::connect(ui_.show_all_covariances, SIGNAL(toggled(bool)), this,
93  SLOT(ShowAllCovariancesToggled(bool)));
94  QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
95  SLOT(ClearPoints()));
96  }
97 
99  {
100  }
101 
103  {
104  ros::master::TopicInfo topic =
105  mapviz::SelectTopicDialog::selectTopic("nav_msgs/Odometry");
106 
107  if (!topic.name.empty())
108  {
109  ui_.topic->setText(QString::fromStdString(topic.name));
110  TopicEdited();
111  }
112  }
113 
115  {
116  std::string topic = ui_.topic->text().trimmed().toStdString();
117  if (topic != topic_)
118  {
119  initialized_ = false;
120  ClearPoints();
121  has_message_ = false;
122  PrintWarning("No messages received.");
123 
125 
126  topic_ = topic;
127  if (!topic.empty())
128  {
131 
132  ROS_INFO("Subscribing to %s", topic_.c_str());
133  }
134  }
135  }
136 
138  const nav_msgs::OdometryConstPtr odometry)
139  {
140  if (!has_message_)
141  {
142  initialized_ = true;
143  has_message_ = true;
144  }
145 
146  // Note that unlike some plugins, this one does not store nor rely on the
147  // source_frame_ member variable. This one can potentially store many
148  // messages with different source frames, so we need to store and transform
149  // them individually.
150  StampedPoint stamped_point;
151  stamped_point.stamp = odometry->header.stamp;
152  stamped_point.source_frame = odometry->header.frame_id;
153 
154  stamped_point.point = tf::Point(odometry->pose.pose.position.x,
155  odometry->pose.pose.position.y,
156  odometry->pose.pose.position.z);
157 
158  stamped_point.orientation = tf::Quaternion(
159  odometry->pose.pose.orientation.x,
160  odometry->pose.pose.orientation.y,
161  odometry->pose.pose.orientation.z,
162  odometry->pose.pose.orientation.w);
163 
164  if ( ui_.show_covariance->isChecked() )
165  {
166  tf::Matrix3x3 tf_cov =
167  swri_transform_util::GetUpperLeft(odometry->pose.covariance);
168 
169  if (tf_cov[0][0] < 100000 && tf_cov[1][1] < 100000)
170  {
171  cv::Mat cov_matrix_3d(3, 3, CV_32FC1);
172  for (int32_t r = 0; r < 3; r++)
173  {
174  for (int32_t c = 0; c < 3; c++)
175  {
176  cov_matrix_3d.at<float>(r, c) = tf_cov[r][c];
177  }
178  }
179 
180  cv::Mat cov_matrix_2d = swri_image_util::ProjectEllipsoid(cov_matrix_3d);
181 
182  if (!cov_matrix_2d.empty())
183  {
185  cov_matrix_2d, stamped_point.point, 3, 32);
186 
187  stamped_point.transformed_cov_points = stamped_point.cov_points;
188  }
189  else
190  {
191  ROS_ERROR("Failed to project x, y, z covariance to xy-plane.");
192  }
193  }
194  }
195 
196  pushPoint( std::move(stamped_point) );
197 
198  }
199 
200  void OdometryPlugin::PrintError(const std::string& message)
201  {
202  PrintErrorHelper(ui_.status, message);
203  }
204 
205  void OdometryPlugin::PrintInfo(const std::string& message)
206  {
207  PrintInfoHelper(ui_.status, message);
208  }
209 
210  void OdometryPlugin::PrintWarning(const std::string& message)
211  {
212  PrintWarningHelper(ui_.status, message);
213  }
214 
215  QWidget* OdometryPlugin::GetConfigWidget(QWidget* parent)
216  {
217  config_widget_->setParent(parent);
218 
219  return config_widget_;
220  }
221 
222  bool OdometryPlugin::Initialize(QGLWidget* canvas)
223  {
224  canvas_ = canvas;
225  SetColor(ui_.color->color());
226 
227  return true;
228  }
229 
230  void OdometryPlugin::Draw(double x, double y, double scale)
231  {
232  if (ui_.show_covariance->isChecked())
233  {
234  DrawCovariance();
235  }
236  if (DrawPoints(scale))
237  {
238  PrintInfo("OK");
239  }
240  }
241 
242 
243  void OdometryPlugin::Paint(QPainter* painter, double x, double y, double scale)
244  {
245  //dont render any timestamps if the show_timestamps is set to 0
246  int interval = ui_.show_timestamps->value();
247  if (interval == 0)
248  {
249  return;
250  }
251 
252  QTransform tf = painter->worldTransform();
253  QFont font("Helvetica", 10);
254  painter->setFont(font);
255  painter->save();
256  painter->resetTransform();
257 
258  //set the draw color for the text to be the same as the rest
259  QPen pen(QBrush(ui_.color->color()), 1);
260  painter->setPen(pen);
261 
262  int counter = 0;//used to alternate between rendering text on some points
263  for (const StampedPoint& point: points())
264  {
265  if (point.transformed && counter % interval == 0)//this renders a timestamp every 'interval' points
266  {
267  QPointF qpoint = tf.map(QPointF(point.transformed_point.getX(),
268  point.transformed_point.getY()));
269  QString time;
270  time.setNum(point.stamp.toSec(), 'g', 12);
271  painter->drawText(qpoint, time);
272  }
273  counter++;
274  }
275 
276  painter->restore();
277  }
278 
279  void OdometryPlugin::LoadConfig(const YAML::Node& node,
280  const std::string& path)
281  {
282  if (node["topic"])
283  {
284  std::string topic;
285  node["topic"] >> topic;
286  ui_.topic->setText(topic.c_str());
287  }
288 
289  if (node["color"])
290  {
291  std::string color;
292  node["color"] >> color;
293  QColor qcolor(color.c_str());
294  SetColor(qcolor);
295  ui_.color->setColor(qcolor);
296  }
297 
298  if (node["draw_style"])
299  {
300  std::string draw_style;
301  node["draw_style"] >> draw_style;
302 
303  if (draw_style == "lines")
304  {
305  ui_.drawstyle->setCurrentIndex(0);
306  SetDrawStyle( LINES );
307  }
308  else if (draw_style == "points")
309  {
310  ui_.drawstyle->setCurrentIndex(1);
311  SetDrawStyle( POINTS );
312  }
313  else if (draw_style == "arrows")
314  {
315  ui_.drawstyle->setCurrentIndex(2);
316  SetDrawStyle( ARROWS );
317  }
318  }
319 
320  if (node["position_tolerance"])
321  {
322  double position_tolerance;
323  node["position_tolerance"] >> position_tolerance;
324  ui_.positiontolerance->setValue(position_tolerance);
325  PositionToleranceChanged(position_tolerance);
326  }
327 
328  if (node["buffer_size"])
329  {
330  double buffer_size;
331  node["buffer_size"] >> buffer_size;
332  ui_.buffersize->setValue(buffer_size);
333  BufferSizeChanged(buffer_size);
334  }
335 
336  if (node["show_covariance"])
337  {
338  bool show_covariance = false;
339  node["show_covariance"] >> show_covariance;
340  ui_.show_covariance->setChecked(show_covariance);
341  CovariancedToggled(show_covariance);
342  }
343 
344  if (node["show_all_covariances"])
345  {
346  bool show_all_covariances = false;
347  node["show_all_covariances"] >> show_all_covariances;
348  ui_.show_all_covariances->setChecked(show_all_covariances);
349  ShowAllCovariancesToggled(show_all_covariances);
350  }
351 
352  if (node["show_laps"])
353  {
354  bool show_laps = false;
355  node["show_laps"] >> show_laps;
356  ui_.show_laps->setChecked(show_laps);
357  LapToggled(show_laps);
358  }
359 
360  if (node["static_arrow_sizes"])
361  {
362  bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
363  ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
364  SetStaticArrowSizes(static_arrow_sizes);
365  }
366 
367  if (node["arrow_size"])
368  {
369  double arrow_size = node["arrow_size"].as<int>();
370  ui_.arrow_size->setValue(arrow_size);
371  SetArrowSize(arrow_size);
372  }
373 
374  if (node["show_timestamps"])
375  {
376  ui_.show_timestamps->setValue(node["show_timestamps"].as<int>());
377  }
378 
379  TopicEdited();
380  }
381 
382  void OdometryPlugin::SaveConfig(YAML::Emitter& emitter,
383  const std::string& path)
384  {
385  std::string topic = ui_.topic->text().toStdString();
386  emitter << YAML::Key << "topic" << YAML::Value << topic;
387 
388  std::string color = ui_.color->color().name().toStdString();
389  emitter << YAML::Key << "color" << YAML::Value << color;
390 
391  std::string draw_style = ui_.drawstyle->currentText().toStdString();
392  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
393 
394  emitter << YAML::Key << "position_tolerance" <<
395  YAML::Value << positionTolerance();
396 
397  emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize();
398 
399  bool show_laps = ui_.show_laps->isChecked();
400  emitter << YAML::Key << "show_laps" << YAML::Value << show_laps;
401 
402  bool show_covariance = ui_.show_covariance->isChecked();
403  emitter << YAML::Key << "show_covariance" << YAML::Value << show_covariance;
404 
405  bool show_all_covariances = ui_.show_all_covariances->isChecked();
406  emitter << YAML::Key << "show_all_covariances" << YAML::Value << show_all_covariances;
407 
408  emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
409 
410  emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
411 
412  emitter << YAML::Key << "show_timestamps" << YAML::Value << ui_.show_timestamps->value();
413  }
414 }
415 
416 
bool Initialize(QGLWidget *canvas)
cv::Mat ProjectEllipsoid(const cv::Mat &ellipsiod)
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
void Draw(double x, double y, double scale)
ros::NodeHandle node_
virtual void PositionToleranceChanged(double value)
virtual void SetArrowSize(int arrowSize)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
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)
void PrintError(const std::string &message)
tf::Vector3 Point
void odometryCallback(const nav_msgs::OdometryConstPtr odometry)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void Paint(QPainter *painter, double x, double y, double scale)
QWidget * GetConfigWidget(QWidget *parent)
void PrintWarning(const std::string &message)
#define ROS_INFO(...)
tf::Matrix3x3 GetUpperLeft(const boost::array< double, 36 > &matrix)
std::vector< tf::Vector3 > GetEllipsePoints(const cv::Mat &ellipse, const tf::Vector3 &center, double scale, int32_t num_points)
virtual void SetColor(const QColor &color)
virtual void SetStaticArrowSizes(bool isChecked)
virtual void ShowAllCovariancesToggled(bool checked)
virtual void CovariancedToggled(bool checked)
void PrintInfo(const std::string &message)
virtual void SetDrawStyle(QString style)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
void LoadConfig(const YAML::Node &node, const std::string &path)
const std::deque< StampedPoint > & points() const


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