pointcloud2_plugin.h
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2015, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #ifndef MAPVIZ_PLUGINS_POINTCLOUD2_PLUGIN_H_
00031 #define MAPVIZ_PLUGINS_POINTCLOUD2_PLUGIN_H_
00032 
00033 // C++ standard libraries
00034 #include <string>
00035 #include <deque>
00036 #include <vector>
00037 #include <map>
00038 
00039 #include <mapviz/mapviz_plugin.h>
00040 
00041 // QT libraries
00042 #include <QGLWidget>
00043 #include <QColor>
00044 #include <QMutex>
00045 
00046 // ROS libraries
00047 #include <sensor_msgs/PointCloud2.h>
00048 
00049 // QT autogenerated files
00050 #include "ui_pointcloud2_config.h"
00051 
00052 namespace mapviz_plugins
00053 {
00054   class PointCloud2Plugin : public mapviz::MapvizPlugin
00055   {
00056   Q_OBJECT
00057 
00058   public:
00059     struct FieldInfo
00060     {
00061       uint8_t datatype;
00062       uint32_t offset;
00063     };
00064 
00065     enum
00066     {
00067       COLOR_FLAT = 0,
00068       COLOR_Z = 3
00069     };
00070 
00071     PointCloud2Plugin();
00072     virtual ~PointCloud2Plugin();
00073 
00074     bool Initialize(QGLWidget* canvas);
00075     void Shutdown()
00076     {
00077     }
00078 
00079     void ClearHistory();
00080 
00081     void Draw(double x, double y, double scale);
00082 
00083     void Transform();
00084 
00085     void LoadConfig(const YAML::Node& node, const std::string& path);
00086     void SaveConfig(YAML::Emitter& emitter, const std::string& path);
00087 
00088     QWidget* GetConfigWidget(QWidget* parent);
00089 
00090   protected:
00091     void PrintError(const std::string& message);
00092     void PrintInfo(const std::string& message);
00093     void PrintWarning(const std::string& message);
00094 
00095   protected Q_SLOTS:
00096     void SelectTopic();
00097     void TopicEdited();
00098     void AlphaEdited(double new_value);
00099     void ColorTransformerChanged(int index);
00100     void MinValueChanged(double value);
00101     void MaxValueChanged(double value);
00102     void PointSizeChanged(int value);
00103     void BufferSizeChanged(int value);
00104     void UseRainbowChanged(int check_state);
00105     void UseAutomaxminChanged(int check_state);
00106     void UpdateColors();
00107     void DrawIcon();
00108     void ResetTransformedPointClouds();
00109 
00110   private:
00111     struct StampedPoint
00112     {
00113       tf::Point point;
00114       tf::Point transformed_point;
00115       QColor color;
00116       std::vector<float> features;
00117     };
00118 
00119     struct Scan
00120     {
00121       ros::Time stamp;
00122       QColor color;
00123       std::vector<StampedPoint> points;
00124       std::string source_frame;
00125       bool transformed;
00126       std::map<std::string, FieldInfo> new_features;
00127     };
00128 
00129     float PointFeature(const uint8_t*, const FieldInfo&);
00130     void PointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& scan);
00131     QColor CalculateColor(const StampedPoint& point);
00132     void UpdateMinMaxWidgets();
00133 
00134     Ui::PointCloud2_config ui_;
00135     QWidget* config_widget_;
00136 
00137     std::string topic_;
00138     double alpha_;
00139     double max_value_;
00140     double min_value_;
00141     size_t point_size_;
00142     size_t buffer_size_;
00143     bool new_topic_;
00144     bool has_message_;
00145     size_t num_of_feats_;
00146     bool need_new_list_;
00147     std::string saved_color_transformer_;
00148     bool need_minmax_;
00149     std::vector<double> max_;
00150     std::vector<double> min_;
00151     // Use a list instead of a deque for scans to facilitate removing
00152     // timed-out scans in the middle of the list in case I ever re-implement
00153     // decay time (evenator)
00154     std::deque<Scan> scans_;
00155     ros::Subscriber pc2_sub_;
00156 
00157     QMutex scan_mutex_;
00158   };
00159 }
00160 
00161 #endif  // MAPVIZ_PLUGINS_LASERSCAN_PLUGIN_H_


mapviz_plugins
Author(s): Marc Alban
autogenerated on Thu Jun 6 2019 18:51:07