gps_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (C) 2013 All Right Reserved, Southwest Research Institute® (SwRI®)
4 //
5 // Contract No. 10-58058A
6 // Contractor Southwest Research Institute® (SwRI®)
7 // Address 6220 Culebra Road, San Antonio, Texas 78228-0510
8 // Contact Steve Dellenback <sdellenback@swri.org> (210) 522-3914
9 //
10 // This code was developed as part of an internal research project fully funded
11 // by Southwest Research Institute®.
12 //
13 // THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
14 // KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
15 // IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
16 // PARTICULAR PURPOSE.
17 //
18 // *****************************************************************************
19 
21 
22 // C++ standard libraries
23 #include <cstdio>
24 #include <vector>
25 
26 // QT libraries
27 #include <QDialog>
28 #include <QGLWidget>
29 #include <QPalette>
30 
31 #include <opencv2/core/core.hpp>
32 
33 // ROS libraries
34 #include <ros/master.h>
35 
39 
40 // Declare plugin
43 
44 namespace mapviz_plugins
45 {
46  GpsPlugin::GpsPlugin() : config_widget_(new QWidget())
47  {
48  ui_.setupUi(config_widget_);
49 
50  ui_.color->setColor(Qt::green);
51 
52  // Set background white
53  QPalette p(config_widget_->palette());
54  p.setColor(QPalette::Background, Qt::white);
55  config_widget_->setPalette(p);
56 
57  // Set status text red
58  QPalette p3(ui_.status->palette());
59  p3.setColor(QPalette::Text, Qt::red);
60  ui_.status->setPalette(p3);
61 
62  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
63  SLOT(SelectTopic()));
64  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
65  SLOT(TopicEdited()));
66  QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
67  SLOT(PositionToleranceChanged(double)));
68  QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this,
69  SLOT(BufferSizeChanged(int)));
70  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
71  SLOT(SetDrawStyle(QString)));
72  QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)),
73  this, SLOT(SetStaticArrowSizes(bool)));
74  QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
75  this, SLOT(SetArrowSize(int)));
76  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
77  SLOT(SetColor(const QColor&)));
78  QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this,
79  SLOT(LapToggled(bool)));
80  QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
81  SLOT(ClearPoints()));
82  }
83 
85  {
86  }
87 
89  {
91  mapviz::SelectTopicDialog::selectTopic("gps_common/GPSFix");
92 
93  if (!topic.name.empty())
94  {
95  ui_.topic->setText(QString::fromStdString(topic.name));
96  TopicEdited();
97  }
98  }
99 
101  {
102  std::string topic = ui_.topic->text().trimmed().toStdString();
103  if (topic != topic_)
104  {
105  initialized_ = false;
106  ClearPoints();
107  has_message_ = false;
108  PrintWarning("No messages received.");
109 
110  gps_sub_.shutdown();
111 
112  topic_ = topic;
113  if (!topic.empty())
114  {
116 
117  ROS_INFO("Subscribing to %s", topic_.c_str());
118  }
119  }
120  }
121 
122  void GpsPlugin::GPSFixCallback(const gps_common::GPSFixConstPtr& gps)
123  {
124  if (!tf_manager_->LocalXyUtil()->Initialized())
125  {
126  return;
127  }
128  if (!has_message_)
129  {
130  initialized_ = true;
131  has_message_ = true;
132  }
133 
134  StampedPoint stamped_point;
135  stamped_point.stamp = gps->header.stamp;
136  stamped_point.source_frame = tf_manager_->LocalXyUtil()->Frame();
137  double x;
138  double y;
139  tf_manager_->LocalXyUtil()->ToLocalXy(gps->latitude, gps->longitude, x, y);
140 
141  stamped_point.point = tf::Point(x, y, gps->altitude);
142 
143  // The GPS "track" is in degrees, but createQuaternionFromYaw expects
144  // radians.
145  // Furthermore, the track rotates in the opposite direction and is also
146  // offset by 90 degrees, so all of that has to be compensated for.
147  stamped_point.orientation =
148  tf::createQuaternionFromYaw((-gps->track * (M_PI / 180.0)) + M_PI_2);
149 
150  pushPoint( std::move( stamped_point) );
151  }
152 
153  void GpsPlugin::PrintError(const std::string& message)
154  {
155  PrintErrorHelper(ui_.status, message);
156  }
157 
158  void GpsPlugin::PrintInfo(const std::string& message)
159  {
160  PrintInfoHelper(ui_.status, message);
161  }
162 
163  void GpsPlugin::PrintWarning(const std::string& message)
164  {
165  PrintWarningHelper(ui_.status, message);
166  }
167 
168  QWidget* GpsPlugin::GetConfigWidget(QWidget* parent)
169  {
170  config_widget_->setParent(parent);
171 
172  return config_widget_;
173  }
174 
175  bool GpsPlugin::Initialize(QGLWidget* canvas)
176  {
177  canvas_ = canvas;
178  SetColor(ui_.color->color());
179 
180  return true;
181  }
182 
183  void GpsPlugin::Draw(double x, double y, double scale)
184  {
185  if (DrawPoints(scale))
186  {
187  PrintInfo("OK");
188  }
189  }
190 
191  void GpsPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
192  {
193  if (node["topic"])
194  {
195  std::string topic;
196  node["topic"] >> topic;
197  ui_.topic->setText(topic.c_str());
198  }
199 
200  if (node["color"])
201  {
202  std::string color;
203  node["color"] >> color;
204  QColor qcolor(color.c_str());
205  SetColor(qcolor);
206  ui_.color->setColor(qcolor);
207  }
208 
209  if (node["draw_style"])
210  {
211  std::string draw_style;
212  node["draw_style"] >> draw_style;
213 
214  if (draw_style == "lines")
215  {
216  ui_.drawstyle->setCurrentIndex(0);
217  SetDrawStyle( LINES );
218  }
219  else if (draw_style == "points")
220  {
221  ui_.drawstyle->setCurrentIndex(1);
222  SetDrawStyle( POINTS );
223  }
224  else if (draw_style == "arrows")
225  {
226  ui_.drawstyle->setCurrentIndex(2);
227  SetDrawStyle( ARROWS );
228  }
229  }
230 
231  if (node["position_tolerance"])
232  {
233  double position_tolerance;
234  node["position_tolerance"] >> position_tolerance;
235  ui_.positiontolerance->setValue(position_tolerance);
236  PositionToleranceChanged(position_tolerance);
237  }
238 
239  if (node["buffer_size"])
240  {
241  double buffer_size;
242  node["buffer_size"] >> buffer_size;
243  ui_.buffersize->setValue(buffer_size);
244  BufferSizeChanged(buffer_size);
245  }
246 
247  if (node["show_laps"])
248  {
249  bool show_laps = false;
250  node["show_laps"] >> show_laps;
251  ui_.show_laps->setChecked(show_laps);
252  LapToggled(show_laps);
253  }
254 
255  if (node["static_arrow_sizes"])
256  {
257  bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
258  ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
259  SetStaticArrowSizes(static_arrow_sizes);
260  }
261 
262  if (node["arrow_size"])
263  {
264  int arrow_size = node["arrow_size"].as<int>();
265  ui_.arrow_size->setValue(arrow_size);
266  SetArrowSize(arrow_size);
267  }
268 
269  TopicEdited();
270  }
271 
272  void GpsPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
273  {
274  std::string topic = ui_.topic->text().toStdString();
275  emitter << YAML::Key << "topic" << YAML::Value << topic;
276 
277  emitter << YAML::Key << "color" << YAML::Value
278  << ui_.color->color().name().toStdString();
279 
280  std::string draw_style = ui_.drawstyle->currentText().toStdString();
281  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
282 
283  emitter << YAML::Key << "position_tolerance" <<
284  YAML::Value << positionTolerance();
285 
286  emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize();
287 
288  bool show_laps = ui_.show_laps->isChecked();
289  emitter << YAML::Key << "show_laps" << YAML::Value << show_laps;
290 
291  emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
292 
293  emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
294  }
295 }
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
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())
QWidget * GetConfigWidget(QWidget *parent)
Definition: gps_plugin.cpp:168
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 LoadConfig(const YAML::Node &node, const std::string &path)
Definition: gps_plugin.cpp:191
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
bool Initialize(QGLWidget *canvas)
Definition: gps_plugin.cpp:175
void GPSFixCallback(const gps_common::GPSFixConstPtr &gps)
Definition: gps_plugin.cpp:122
static Quaternion createQuaternionFromYaw(double yaw)
Ui::gps_config ui_
Definition: gps_plugin.h:80
#define ROS_INFO(...)
void Draw(double x, double y, double scale)
Definition: gps_plugin.cpp:183
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
Definition: gps_plugin.cpp:272
virtual void SetColor(const QColor &color)
virtual void SetStaticArrowSizes(bool isChecked)
void PrintWarning(const std::string &message)
Definition: gps_plugin.cpp:163
ros::Subscriber gps_sub_
Definition: gps_plugin.h:85
void PrintInfo(const std::string &message)
Definition: gps_plugin.cpp:158
void PrintError(const std::string &message)
Definition: gps_plugin.cpp:153
virtual void SetDrawStyle(QString style)
swri_transform_util::TransformManagerPtr tf_manager_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


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