mapviz_plugin.h
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 
30 #ifndef MAPVIZ_MAPVIZ_PLUGIN_H_
31 #define MAPVIZ_MAPVIZ_PLUGIN_H_
32 
33 // C++ standard libraries
34 #include <string>
35 
36 #include <boost/make_shared.hpp>
37 #include <boost/shared_ptr.hpp>
38 
39 // QT libraries
40 #include <QWidget>
41 #include <QGLWidget>
42 #include <QObject>
43 
44 // ROS libraries
45 #include <ros/ros.h>
46 #include <tf/transform_datatypes.h>
50 
51 #include <mapviz/widgets.h>
52 
53 #include "stopwatch.h"
54 
55 namespace mapviz
56 {
57  class MapvizPlugin : public QObject
58  {
59  Q_OBJECT;
60  public:
61  virtual ~MapvizPlugin() {}
62 
63  virtual bool Initialize(
66  QGLWidget* canvas)
67  {
68  tf_ = tf_listener;
69  tf_manager_ = tf_manager;
70  return Initialize(canvas);
71  }
72 
73  virtual void Shutdown() = 0;
74 
75  virtual void ClearHistory() {}
76 
81  virtual void Draw(double x, double y, double scale) = 0;
82 
87  virtual void Paint(QPainter* painter, double x, double y, double scale) {};
88 
89  void SetUseLatestTransforms(bool value)
90  {
91  if (value != use_latest_transforms_)
92  {
93  use_latest_transforms_ = value;
95  }
96  }
97 
98  void SetName(const std::string& name) { name_ = name; }
99 
100  std::string Name() const { return name_; }
101 
102  void SetType(const std::string& type) { type_ = type; }
103 
104  std::string Type() const { return type_; }
105 
106  int DrawOrder() const { return draw_order_; }
107 
108  void SetDrawOrder(int order)
109  {
110  if (draw_order_ != order)
111  {
112  draw_order_ = order;
114  }
115  }
116 
117  virtual void SetNode(const ros::NodeHandle& node)
118  {
119  node_ = node;
120  }
121 
122  void DrawPlugin(double x, double y, double scale)
123  {
124  if (visible_ && initialized_)
125  {
127  Transform();
129 
130  meas_draw_.start();
131  Draw(x, y, scale);
132  meas_draw_.stop();
133  }
134  }
135 
136  void PaintPlugin(QPainter* painter, double x, double y, double scale)
137  {
138  if (visible_ && initialized_)
139  {
141  Transform();
143 
144  meas_paint_.start();
145  Paint(painter, x, y, scale);
146  meas_paint_.start();
147  }
148  }
149 
150  void SetTargetFrame(std::string frame_id)
151  {
152  if (frame_id != target_frame_)
153  {
154  target_frame_ = frame_id;
155 
157  Transform();
159 
161  }
162  }
163 
164  bool Visible() const { return visible_; }
165 
166  void SetVisible(bool visible)
167  {
168  if (visible_ != visible)
169  {
170  visible_ = visible;
171  Q_EMIT VisibleChanged(visible_);
172  }
173  }
174 
175  bool GetTransform(const ros::Time& stamp, swri_transform_util::Transform& transform, bool use_latest_transforms = true)
176  {
177  if (!initialized_)
178  return false;
179 
180  ros::Time time = stamp;
181 
182  if (use_latest_transforms_ && use_latest_transforms)
183  {
184  time = ros::Time();
185  }
186 
187  ros::Duration elapsed = ros::Time::now() - time;
188 
189  if (time != ros::Time() && elapsed > tf_->getCacheLength())
190  {
191  return false;
192  }
193 
194  if (tf_manager_->GetTransform(target_frame_, source_frame_, time, transform))
195  {
196  return true;
197  }
198  else if (elapsed.toSec() < 0.1)
199  {
200  // If the stamped transform failed because it is too recent, find the
201  // most recent transform in the cache instead.
202  if (tf_manager_->GetTransform(target_frame_, source_frame_, ros::Time(), transform))
203  {
204  return true;
205  }
206  }
207 
208  return false;
209  }
210 
211  bool GetTransform(const std::string& source, const ros::Time& stamp, swri_transform_util::Transform& transform)
212  {
213  if (!initialized_)
214  return false;
215 
216  ros::Time time = stamp;
217 
219  {
220  time = ros::Time();
221  }
222 
223  ros::Duration elapsed = ros::Time::now() - time;
224 
225  if (time != ros::Time() && elapsed > tf_->getCacheLength())
226  {
227  return false;
228  }
229 
230  if (tf_manager_->GetTransform(target_frame_, source, time, transform))
231  {
232  return true;
233  }
234  else if (elapsed.toSec() < 0.1)
235  {
236  // If the stamped transform failed because it is too recent, find the
237  // most recent transform in the cache instead.
238  if (tf_manager_->GetTransform(target_frame_, source, ros::Time(), transform))
239  {
240  return true;
241  }
242  }
243 
244  return false;
245  }
246 
247  virtual void Transform() = 0;
248 
249  virtual void LoadConfig(const YAML::Node& load, const std::string& path) = 0;
250  virtual void SaveConfig(YAML::Emitter& emitter, const std::string& path) = 0;
251 
252  virtual QWidget* GetConfigWidget(QWidget* parent) { return NULL; }
253 
254  virtual void PrintError(const std::string& message) = 0;
255  virtual void PrintInfo(const std::string& message) = 0;
256  virtual void PrintWarning(const std::string& message) = 0;
257 
258  void SetIcon(IconWidget* icon) { icon_ = icon; }
259 
261  {
262  std::string header = type_ + " (" + name_ + ")";
263  meas_transform_.printInfo(header + " Transform()");
264  meas_paint_.printInfo(header + " Paint()");
265  meas_draw_.printInfo(header + " Draw()");
266  }
267 
268  static void PrintErrorHelper(QLabel *status_label, const std::string& message, double throttle = 0.0);
269  static void PrintInfoHelper(QLabel *status_label, const std::string& message, double throttle = 0.0);
270  static void PrintWarningHelper(QLabel *status_label, const std::string& message, double throttle = 0.0);
271 
272  public Q_SLOTS:
273  virtual void DrawIcon() {}
274 
279  virtual bool SupportsPainting()
280  {
281  return false;
282  }
283 
284  Q_SIGNALS:
285  void DrawOrderChanged(int draw_order);
286  void SizeChanged();
287  void TargetFrameChanged(const std::string& target_frame);
288  void UseLatestTransformsChanged(bool use_latest_transforms);
289  void VisibleChanged(bool visible);
290 
291 
292  protected:
294  bool visible_;
295 
296  QGLWidget* canvas_;
298 
300 
303 
304  std::string target_frame_;
305  std::string source_frame_;
306  std::string type_;
307  std::string name_;
308 
310 
312 
313  virtual bool Initialize(QGLWidget* canvas) = 0;
314 
316  initialized_(false),
317  visible_(true),
318  canvas_(NULL),
319  icon_(NULL),
320  tf_(),
321  target_frame_(""),
322  source_frame_(""),
323  use_latest_transforms_(false),
324  draw_order_(0) {}
325 
326  private:
327  // Collect basic profiling info to know how much time each plugin
328  // spends in Transform(), Paint(), and Draw().
332  };
334 
335  // Implementation
336 
337  inline void MapvizPlugin::PrintErrorHelper(QLabel *status_label, const std::string &message,
338  double throttle)
339  {
340  if (message == status_label->text().toStdString())
341  {
342  return;
343  }
344 
345  if( throttle > 0.0){
346  ROS_ERROR_THROTTLE(throttle, "Error: %s", message.c_str());
347  }
348  else{
349  ROS_ERROR("Error: %s", message.c_str());
350  }
351  QPalette p(status_label->palette());
352  p.setColor(QPalette::Text, Qt::red);
353  status_label->setPalette(p);
354  status_label->setText(message.c_str());
355  }
356 
357  inline void MapvizPlugin::PrintInfoHelper(QLabel *status_label, const std::string &message,
358  double throttle)
359  {
360  if (message == status_label->text().toStdString())
361  {
362  return;
363  }
364 
365  if( throttle > 0.0){
366  ROS_INFO_THROTTLE(throttle, "%s", message.c_str());
367  }
368  else{
369  ROS_INFO("%s", message.c_str());
370  }
371  QPalette p(status_label->palette());
372  p.setColor(QPalette::Text, Qt::darkGreen);
373  status_label->setPalette(p);
374  status_label->setText(message.c_str());
375  }
376 
377  inline void MapvizPlugin::PrintWarningHelper(QLabel *status_label, const std::string &message,
378  double throttle)
379  {
380  if (message == status_label->text().toStdString())
381  {
382  return;
383  }
384 
385  if( throttle > 0.0){
386  ROS_WARN_THROTTLE(throttle, "%s", message.c_str());
387  }
388  else{
389  ROS_WARN("%s", message.c_str());
390  }
391  QPalette p(status_label->palette());
392  p.setColor(QPalette::Text, Qt::darkYellow);
393  status_label->setPalette(p);
394  status_label->setText(message.c_str());
395  }
396 
397 }
398 #endif // MAPVIZ_MAPVIZ_PLUGIN_H_
399 
virtual void Shutdown()=0
#define NULL
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
void SetTargetFrame(std::string frame_id)
bool Visible() const
#define ROS_WARN_THROTTLE(rate,...)
ros::NodeHandle node_
void printInfo(const std::string &name) const
Definition: stopwatch.h:87
int DrawOrder() const
virtual bool SupportsPainting()
std::string Name() const
virtual void PrintWarning(const std::string &message)=0
virtual void SetNode(const ros::NodeHandle &node)
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 SetName(const std::string &name)
Definition: mapviz_plugin.h:98
void SetVisible(bool visible)
void PaintPlugin(QPainter *painter, double x, double y, double scale)
std::string target_frame_
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
std::string Type() const
bool GetTransform(const std::string &source, const ros::Time &stamp, swri_transform_util::Transform &transform)
std::string source_frame_
void SetType(const std::string &type)
#define ROS_ERROR_THROTTLE(rate,...)
virtual void PrintInfo(const std::string &message)=0
#define ROS_INFO(...)
virtual void Paint(QPainter *painter, double x, double y, double scale)
Definition: mapviz_plugin.h:87
virtual bool Initialize(boost::shared_ptr< tf::TransformListener > tf_listener, swri_transform_util::TransformManagerPtr tf_manager, QGLWidget *canvas)
Definition: mapviz_plugin.h:63
void TargetFrameChanged(const std::string &target_frame)
void DrawPlugin(double x, double y, double scale)
boost::shared_ptr< MapvizPlugin > MapvizPluginPtr
TFSIMD_FORCE_INLINE const tfScalar & x() const
void SetIcon(IconWidget *icon)
virtual void ClearHistory()
Definition: mapviz_plugin.h:75
void SetUseLatestTransforms(bool value)
Definition: mapviz_plugin.h:89
void UseLatestTransformsChanged(bool use_latest_transforms)
virtual void Draw(double x, double y, double scale)=0
void VisibleChanged(bool visible)
virtual void DrawIcon()
virtual void LoadConfig(const YAML::Node &load, const std::string &path)=0
static Time now()
#define ROS_INFO_THROTTLE(rate,...)
void SetDrawOrder(int order)
boost::shared_ptr< tf::TransformListener > tf_
swri_transform_util::TransformManagerPtr tf_manager_
void DrawOrderChanged(int draw_order)
#define ROS_ERROR(...)
virtual QWidget * GetConfigWidget(QWidget *parent)
virtual void SaveConfig(YAML::Emitter &emitter, const std::string &path)=0
def load(fp)
virtual void PrintError(const std::string &message)=0
virtual void Transform()=0


mapviz
Author(s): Marc Alban
autogenerated on Fri Mar 19 2021 02:44:25