pointcloud2_plugin.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2015, 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_PLUGINS_POINTCLOUD2_PLUGIN_H_
31 #define MAPVIZ_PLUGINS_POINTCLOUD2_PLUGIN_H_
32 
33 // C++ standard libraries
34 #include <string>
35 #include <deque>
36 #include <vector>
37 #include <map>
38 
39 #include <mapviz/mapviz_plugin.h>
40 
41 // QT libraries
42 #include <QGLWidget>
43 #include <QColor>
44 #include <QMutex>
45 
46 // ROS libraries
47 #include <sensor_msgs/PointCloud2.h>
48 
49 // QT autogenerated files
50 #include "ui_pointcloud2_config.h"
51 
52 namespace mapviz_plugins
53 {
55  {
56  Q_OBJECT
57 
58  public:
59  struct FieldInfo
60  {
61  uint8_t datatype;
62  uint32_t offset;
63  };
64 
65  enum
66  {
68  COLOR_Z = 3
69  };
70 
72  virtual ~PointCloud2Plugin();
73 
74  bool Initialize(QGLWidget* canvas);
75  void Shutdown()
76  {
77  }
78 
79  void ClearHistory();
80 
81  void Draw(double x, double y, double scale);
82 
83  void Transform();
84 
85  void LoadConfig(const YAML::Node& node, const std::string& path);
86  void SaveConfig(YAML::Emitter& emitter, const std::string& path);
87 
88  QWidget* GetConfigWidget(QWidget* parent);
89 
90  protected:
91  void PrintError(const std::string& message);
92  void PrintInfo(const std::string& message);
93  void PrintWarning(const std::string& message);
94 
95  protected Q_SLOTS:
96  void SelectTopic();
97  void TopicEdited();
98  void AlphaEdited(double new_value);
99  void ColorTransformerChanged(int index);
100  void MinValueChanged(double value);
101  void MaxValueChanged(double value);
102  void PointSizeChanged(int value);
103  void BufferSizeChanged(int value);
104  void UseRainbowChanged(int check_state);
105  void UseAutomaxminChanged(int check_state);
106  void UpdateColors();
107  void DrawIcon();
109  void ClearPointClouds();
110  void SetSubscription(bool subscribe);
111 
112  private:
114  {
116  std::vector<float> features;
117  };
118 
119  struct Scan
120  {
122  QColor color;
123  std::vector<StampedPoint> points;
124  std::string source_frame;
126  std::map<std::string, FieldInfo> new_features;
127 
128  std::vector<float> gl_point;
129  std::vector<uint8_t> gl_color;
130  GLuint point_vbo;
131  GLuint color_vbo;
132  };
133 
134  float PointFeature(const uint8_t*, const FieldInfo&);
135  void PointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& scan);
136  QColor CalculateColor(const StampedPoint& point);
137  void UpdateMinMaxWidgets();
138 
139  Ui::PointCloud2_config ui_;
140  QWidget* config_widget_;
141 
142  std::string topic_;
143  double alpha_;
144  double max_value_;
145  double min_value_;
146  size_t point_size_;
147  size_t buffer_size_;
154  std::vector<double> max_;
155  std::vector<double> min_;
156  // Use a list instead of a deque for scans to facilitate removing
157  // timed-out scans in the middle of the list in case I ever re-implement
158  // decay time (evenator)
159  std::deque<Scan> scans_;
161 
162  QMutex scan_mutex_;
163  };
164 }
165 
166 #endif // MAPVIZ_PLUGINS_LASERSCAN_PLUGIN_H_
QColor CalculateColor(const StampedPoint &point)
void Draw(double x, double y, double scale)
void PointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr &scan)
tf::Vector3 Point
void PrintError(const std::string &message)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void PrintInfo(const std::string &message)
void LoadConfig(const YAML::Node &node, const std::string &path)
swri::Subscriber subscribe(swri::NodeHandle &nh, const std::string &name, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &), T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
QWidget * GetConfigWidget(QWidget *parent)
std::map< std::string, FieldInfo > new_features
float PointFeature(const uint8_t *, const FieldInfo &)
void UseAutomaxminChanged(int check_state)
void PrintWarning(const std::string &message)


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