pose_plugin.cpp
Go to the documentation of this file.
1 
32 
33 // C++ standard libraries
34 #include <cstdio>
35 #include <vector>
36 
37 // QT libraries
38 #include <QDialog>
39 #include <QGLWidget>
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  PosePlugin::PosePlugin() : config_widget_(new QWidget())
58  {
59  ui_.setupUi(config_widget_);
60 
61  ui_.color->setColor(Qt::green);
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_.selecttopic, SIGNAL(clicked()), this,
74  SLOT(SelectTopic()));
75  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
76  SLOT(TopicEdited()));
77  QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
78  SLOT(PositionToleranceChanged(double)));
79  QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this,
80  SLOT(BufferSizeChanged(int)));
81  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
82  SLOT(SetDrawStyle(QString)));
83  QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)),
84  this, SLOT(SetStaticArrowSizes(bool)));
85  QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
86  this, SLOT(SetArrowSize(int)));
87  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
88  SLOT(SetColor(const QColor&)));
89  QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this,
90  SLOT(LapToggled(bool)));
91  QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
92  SLOT(ClearPoints()));
93  }
94 
96  {
97  }
98 
100  {
101  ros::master::TopicInfo topic =
102  mapviz::SelectTopicDialog::selectTopic("geometry_msgs/PoseStamped");
103 
104  if (!topic.name.empty())
105  {
106  ui_.topic->setText(QString::fromStdString(topic.name));
107  TopicEdited();
108  }
109  }
110 
112  {
113  std::string topic = ui_.topic->text().trimmed().toStdString();
114  if (topic != topic_)
115  {
116  initialized_ = false;
117  ClearPoints();
118  has_message_ = false;
119  PrintWarning("No messages received.");
120 
122 
123  topic_ = topic;
124  if (!topic.empty())
125  {
127 
128  ROS_INFO("Subscribing to %s", topic_.c_str());
129  }
130  }
131  }
132 
133  void PosePlugin::PoseCallback(const geometry_msgs::PoseStampedConstPtr& pose)
134  {
135  if (!has_message_)
136  {
137  initialized_ = true;
138  has_message_ = true;
139  }
140 
141  StampedPoint stamped_point;
142  stamped_point.stamp = pose->header.stamp;
143  stamped_point.source_frame = pose->header.frame_id;
144 
145  stamped_point.point = tf::Point(pose->pose.position.x,
146  pose->pose.position.y,
147  pose->pose.position.z);
148 
149  stamped_point.orientation = tf::Quaternion(
150  pose->pose.orientation.x,
151  pose->pose.orientation.y,
152  pose->pose.orientation.z,
153  pose->pose.orientation.w);
154 
155  pushPoint( std::move( stamped_point) );
156  }
157 
158  void PosePlugin::PrintError(const std::string& message)
159  {
160  PrintErrorHelper(ui_.status, message);
161  }
162 
163  void PosePlugin::PrintInfo(const std::string& message)
164  {
165  PrintInfoHelper(ui_.status, message);
166  }
167 
168  void PosePlugin::PrintWarning(const std::string& message)
169  {
170  PrintWarningHelper(ui_.status, message);
171  }
172 
173  QWidget* PosePlugin::GetConfigWidget(QWidget* parent)
174  {
175  config_widget_->setParent(parent);
176 
177  return config_widget_;
178  }
179 
180  bool PosePlugin::Initialize(QGLWidget* canvas)
181  {
182  canvas_ = canvas;
183  SetColor(ui_.color->color());
184 
185  return true;
186  }
187 
188  void PosePlugin::Draw(double x, double y, double scale)
189  {
190  if (DrawPoints(scale))
191  {
192  PrintInfo("OK");
193  }
194  }
195 
196  void PosePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
197  {
198  if (node["topic"])
199  {
200  std::string topic;
201  node["topic"] >> topic;
202  ui_.topic->setText(topic.c_str());
203  }
204 
205  if (node["color"])
206  {
207  std::string color;
208  node["color"] >> color;
209  QColor qcolor(color.c_str());
210  SetColor(qcolor);
211  ui_.color->setColor(qcolor);
212  }
213 
214  if (node["draw_style"])
215  {
216  std::string draw_style;
217  node["draw_style"] >> draw_style;
218 
219  if (draw_style == "lines")
220  {
221  ui_.drawstyle->setCurrentIndex(0);
222  SetDrawStyle( LINES );
223  }
224  else if (draw_style == "points")
225  {
226  ui_.drawstyle->setCurrentIndex(1);
227  SetDrawStyle( POINTS );
228  }
229  else if (draw_style == "arrows")
230  {
231  ui_.drawstyle->setCurrentIndex(2);
232  SetDrawStyle( ARROWS );
233  }
234  }
235 
236  if (node["position_tolerance"])
237  {
238  double position_tolerance;
239  node["position_tolerance"] >> position_tolerance;
240  ui_.positiontolerance->setValue(position_tolerance);
241  PositionToleranceChanged(position_tolerance);
242  }
243 
244  if (node["buffer_size"])
245  {
246  double buffer_size;
247  node["buffer_size"] >> buffer_size;
248  ui_.buffersize->setValue(buffer_size);
249  BufferSizeChanged(buffer_size);
250  }
251 
252  if (node["show_laps"])
253  {
254  bool show_laps = false;
255  node["show_laps"] >> show_laps;
256  ui_.show_laps->setChecked(show_laps);
257  LapToggled(show_laps);
258  }
259 
260  if (node["static_arrow_sizes"])
261  {
262  bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
263  ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
264  SetStaticArrowSizes(static_arrow_sizes);
265  }
266 
267  if (node["arrow_size"])
268  {
269  int arrow_size = node["arrow_size"].as<int>();
270  ui_.arrow_size->setValue(arrow_size);
271  SetArrowSize(arrow_size);
272  }
273 
274  TopicEdited();
275  }
276 
277  void PosePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
278  {
279  std::string topic = ui_.topic->text().toStdString();
280  emitter << YAML::Key << "topic" << YAML::Value << topic;
281 
282  emitter << YAML::Key << "color" << YAML::Value
283  << ui_.color->color().name().toStdString();
284 
285  std::string draw_style = ui_.drawstyle->currentText().toStdString();
286  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
287 
288  emitter << YAML::Key << "position_tolerance" <<
289  YAML::Value << positionTolerance();
290 
291  emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize();
292 
293  bool show_laps = ui_.show_laps->isChecked();
294  emitter << YAML::Key << "show_laps" << YAML::Value << show_laps;
295 
296  emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
297 
298  emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
299  }
300 }
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())
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)
void PrintInfo(const std::string &message)
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
#define ROS_INFO(...)
void Draw(double x, double y, double scale)
QWidget * GetConfigWidget(QWidget *parent)
virtual void SetColor(const QColor &color)
virtual void SetStaticArrowSizes(bool isChecked)
ros::Subscriber pose_sub_
Definition: pose_plugin.h:96
void PoseCallback(const geometry_msgs::PoseStampedConstPtr &pose)
virtual void SetDrawStyle(QString style)
bool Initialize(QGLWidget *canvas)
void PrintWarning(const std::string &message)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void LoadConfig(const YAML::Node &node, const std::string &path)


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