MapCloudDisplay.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #ifndef MAP_CLOUD_DISPLAY_H
00029 #define MAP_CLOUD_DISPLAY_H
00030 
00031 #ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
00032 
00033 #include <deque>
00034 #include <queue>
00035 #include <vector>
00036 
00037 #include <rtabmap_ros/MapData.h>
00038 #include <rtabmap/core/Transform.h>
00039 
00040 #include <pluginlib/class_loader.h>
00041 #include <sensor_msgs/PointCloud2.h>
00042 
00043 #include <rviz/ogre_helpers/point_cloud.h>
00044 #include <rviz/message_filter_display.h>
00045 #include <rviz/default_plugin/point_cloud_transformer.h>
00046 
00047 #endif
00048 
00049 namespace rviz {
00050 class IntProperty;
00051 class BoolProperty;
00052 class EnumProperty;
00053 class FloatProperty;
00054 class PointCloudTransformer;
00055 typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
00056 typedef std::vector<std::string> V_string;
00057 }
00058 
00059 using namespace rviz;
00060 
00061 namespace rtabmap_ros
00062 {
00063 
00064 class PointCloudCommon;
00065 
00074 class MapCloudDisplay: public rviz::MessageFilterDisplay<rtabmap_ros::MapData>
00075 {
00076 Q_OBJECT
00077 public:
00078         struct CloudInfo
00079           {
00080                 CloudInfo();
00081                 ~CloudInfo();
00082 
00083                 // clear the point cloud, but keep selection handler around
00084                 void clear();
00085 
00086                 Ogre::SceneManager *manager_;
00087 
00088                 sensor_msgs::PointCloud2ConstPtr message_;
00089                 rtabmap::Transform pose_;
00090                 int id_;
00091 
00092                 Ogre::SceneNode *scene_node_;
00093                 boost::shared_ptr<rviz::PointCloud> cloud_;
00094 
00095                 std::vector<rviz::PointCloud::Point> transformed_points_;
00096         };
00097         typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
00098 
00099         MapCloudDisplay();
00100         virtual ~MapCloudDisplay();
00101 
00102         virtual void reset();
00103         virtual void update( float wall_dt, float ros_dt );
00104 
00105         rviz::FloatProperty* point_world_size_property_;
00106         rviz::FloatProperty* point_pixel_size_property_;
00107         rviz::FloatProperty* alpha_property_;
00108         rviz::EnumProperty* xyz_transformer_property_;
00109         rviz::EnumProperty* color_transformer_property_;
00110         rviz::EnumProperty* style_property_;
00111         rviz::IntProperty* cloud_decimation_;
00112         rviz::FloatProperty* cloud_max_depth_;
00113         rviz::FloatProperty* cloud_min_depth_;
00114         rviz::FloatProperty* cloud_voxel_size_;
00115         rviz::FloatProperty* cloud_filter_floor_height_;
00116         rviz::FloatProperty* cloud_filter_ceiling_height_;
00117         rviz::FloatProperty* node_filtering_radius_;
00118         rviz::FloatProperty* node_filtering_angle_;
00119         rviz::BoolProperty* download_map_;
00120         rviz::BoolProperty* download_graph_;
00121 
00122 public Q_SLOTS:
00123         void causeRetransform();
00124 
00125 private Q_SLOTS:
00126         void updateStyle();
00127         void updateBillboardSize();
00128         void updateAlpha();
00129         void updateXyzTransformer();
00130         void updateColorTransformer();
00131         void setXyzTransformerOptions( EnumProperty* prop );
00132         void setColorTransformerOptions( EnumProperty* prop );
00133         void updateCloudParameters();
00134         void downloadMap();
00135         void downloadGraph();
00136 
00137 protected:
00139         virtual void onInitialize();
00140 
00142         virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud );
00143 
00144 private:
00145         void processMapData(const rtabmap_ros::MapData& map);
00146 
00150         bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
00151 
00152         rviz::PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00153         rviz::PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00154         void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
00155         void retransform();
00156 
00157         void loadTransformers();
00158 
00159         void setPropertiesHidden( const QList<Property*>& props, bool hide );
00160         void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask );
00161 
00162 private:
00163         ros::AsyncSpinner spinner_;
00164         ros::CallbackQueue cbqueue_;
00165 
00166         std::map<int, CloudInfoPtr> cloud_infos_;
00167 
00168         std::map<int, CloudInfoPtr> new_cloud_infos_;
00169         boost::mutex new_clouds_mutex_;
00170 
00171         std::map<int, rtabmap::Transform> current_map_;
00172         boost::mutex current_map_mutex_;
00173 
00174         struct TransformerInfo
00175         {
00176                 rviz::PointCloudTransformerPtr transformer;
00177                 QList<Property*> xyz_props;
00178                 QList<Property*> color_props;
00179 
00180                 std::string readable_name;
00181                 std::string lookup_name;
00182         };
00183         typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
00184 
00185         boost::recursive_mutex transformers_mutex_;
00186         M_TransformerInfo transformers_;
00187         bool new_xyz_transformer_;
00188         bool new_color_transformer_;
00189         bool needs_retransform_;
00190 
00191         pluginlib::ClassLoader<rviz::PointCloudTransformer>* transformer_class_loader_;
00192 };
00193 
00194 } // namespace rtabmap_ros
00195 
00196 #endif


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:08