navsat_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 
38 
40 
41 // Declare plugin
44 
45 namespace mapviz_plugins
46 {
47  NavSatPlugin::NavSatPlugin() : config_widget_(new QWidget())
48  {
49  ui_.setupUi(config_widget_);
50 
51  ui_.color->setColor(Qt::green);
52 
53  // Set background white
54  QPalette p(config_widget_->palette());
55  p.setColor(QPalette::Background, Qt::white);
56  config_widget_->setPalette(p);
57 
58  // Set status text red
59  QPalette p3(ui_.status->palette());
60  p3.setColor(QPalette::Text, Qt::red);
61  ui_.status->setPalette(p3);
62 
63  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
64  SLOT(SelectTopic()));
65  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
66  SLOT(TopicEdited()));
67  QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
68  SLOT(PositionToleranceChanged(double)));
69  QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this,
70  SLOT(BufferSizeChanged(int)));
71  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
72  SLOT(SetDrawStyle(QString)));
73  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
74  SLOT(SetColor(const QColor&)));
75  QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
76  SLOT(ClearPoints()));
77  }
78 
80  {
81  }
82 
84  {
86  mapviz::SelectTopicDialog::selectTopic("sensor_msgs/NavSatFix");
87 
88  if (!topic.name.empty())
89  {
90  ui_.topic->setText(QString::fromStdString(topic.name));
91  TopicEdited();
92  }
93  }
94 
96  {
97  std::string topic = ui_.topic->text().trimmed().toStdString();
98  if (topic != topic_)
99  {
100  initialized_ = false;
101  ClearPoints();
102  has_message_ = false;
103  PrintWarning("No messages received.");
104 
106  topic_ = topic;
107  if (!topic.empty())
108  {
110 
111  ROS_INFO("Subscribing to %s", topic_.c_str());
112  }
113  }
114  }
115 
117  const sensor_msgs::NavSatFixConstPtr navsat)
118  {
119  if (!tf_manager_->LocalXyUtil()->Initialized())
120  {
121  return;
122  }
123  if (!has_message_)
124  {
125  initialized_ = true;
126  has_message_ = true;
127  }
128 
129  StampedPoint stamped_point;
130  stamped_point.stamp = navsat->header.stamp;
131 
132  double x;
133  double y;
134  tf_manager_->LocalXyUtil()->ToLocalXy(navsat->latitude, navsat->longitude, x, y);
135 
136  stamped_point.point = tf::Point(x, y, navsat->altitude);
137  stamped_point.orientation = tf::createQuaternionFromYaw(0.0);
138  stamped_point.source_frame = tf_manager_->LocalXyUtil()->Frame();
139 
140  pushPoint( std::move(stamped_point ) );
141  }
142 
143  void NavSatPlugin::PrintError(const std::string& message)
144  {
145  PrintErrorHelper(ui_.status, message);
146  }
147 
148  void NavSatPlugin::PrintInfo(const std::string& message)
149  {
150  PrintInfoHelper(ui_.status, message);
151  }
152 
153  void NavSatPlugin::PrintWarning(const std::string& message)
154  {
155  PrintWarningHelper(ui_.status, message);
156  }
157 
158  QWidget* NavSatPlugin::GetConfigWidget(QWidget* parent)
159  {
160  config_widget_->setParent(parent);
161 
162  return config_widget_;
163  }
164 
165  bool NavSatPlugin::Initialize(QGLWidget* canvas)
166  {
167  canvas_ = canvas;
168  SetColor(ui_.color->color());
169  return true;
170  }
171 
172  void NavSatPlugin::Draw(double x, double y, double scale)
173  {
174  if (DrawPoints(scale))
175  {
176  PrintInfo("OK");
177  }
178  }
179 
180  void NavSatPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
181  {
182  if (node["topic"])
183  {
184  std::string topic;
185  node["topic"] >> topic;
186  ui_.topic->setText(topic.c_str());
187  }
188 
189  if (node["color"])
190  {
191  std::string color;
192  node["color"] >> color;
193  QColor qcolor(color.c_str());
194  SetColor(qcolor);
195  ui_.color->setColor(qcolor);
196  }
197 
198  if (node["draw_style"])
199  {
200  std::string draw_style;
201  node["draw_style"] >> draw_style;
202 
203  if (draw_style == "lines")
204  {
205  ui_.drawstyle->setCurrentIndex(0);
206  SetDrawStyle( LINES );
207  }
208  else if (draw_style == "points")
209  {
210  ui_.drawstyle->setCurrentIndex(1);
211  SetDrawStyle( POINTS );
212  }
213  }
214 
215  if (node["position_tolerance"])
216  {
217  double position_tolerance;
218  node["position_tolerance"] >> position_tolerance;
219  ui_.positiontolerance->setValue(position_tolerance);
220  PositionToleranceChanged(position_tolerance);
221  }
222 
223  if (node["buffer_size"])
224  {
225  double buffer_size;
226  node["buffer_size"] >> buffer_size;
227  ui_.buffersize->setValue(buffer_size);
228  BufferSizeChanged(buffer_size);
229  }
230 
231  TopicEdited();
232  }
233 
234  void NavSatPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
235  {
236  std::string topic = ui_.topic->text().toStdString();
237  emitter << YAML::Key << "topic" << YAML::Value << topic;
238 
239  std::string color = ui_.color->color().name().toStdString();
240  emitter << YAML::Key << "color" << YAML::Value << color;
241 
242  std::string draw_style = ui_.drawstyle->currentText().toStdString();
243  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
244 
245  emitter << YAML::Key << "position_tolerance" <<
246  YAML::Value << positionTolerance();
247 
248  emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize();
249  }
250 }
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
void PrintInfo(const std::string &message)
ros::NodeHandle node_
virtual void PositionToleranceChanged(double value)
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)
QWidget * GetConfigWidget(QWidget *parent)
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void Draw(double x, double y, double scale)
static Quaternion createQuaternionFromYaw(double yaw)
void NavSatFixCallback(const sensor_msgs::NavSatFixConstPtr navsat)
#define ROS_INFO(...)
virtual void SetColor(const QColor &color)
void PrintWarning(const std::string &message)
void LoadConfig(const YAML::Node &node, const std::string &path)
bool Initialize(QGLWidget *canvas)
virtual void SetDrawStyle(QString style)
void PrintError(const std::string &message)
swri_transform_util::TransformManagerPtr tf_manager_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)


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