point_cloud_common.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
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  *
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 the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       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 THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef RVIZ_POINT_CLOUD_COMMON_H
00031 #define RVIZ_POINT_CLOUD_COMMON_H
00032 
00033 #ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
00034 # include <deque>
00035 # include <queue>
00036 # include <vector>
00037 
00038 # include <QObject>
00039 # include <QList>
00040 
00041 # include <boost/shared_ptr.hpp>
00042 # include <boost/thread/mutex.hpp>
00043 # include <boost/thread/recursive_mutex.hpp>
00044 
00045 # include <ros/spinner.h>
00046 # include <ros/callback_queue.h>
00047 
00048 # include <message_filters/time_sequencer.h>
00049 
00050 # include <pluginlib/class_loader.h>
00051 
00052 # include <sensor_msgs/PointCloud.h>
00053 # include <sensor_msgs/PointCloud2.h>
00054 
00055 # include "rviz/selection/selection_manager.h"
00056 # include "rviz/default_plugin/point_cloud_transformer.h"
00057 # include "rviz/properties/color_property.h"
00058 # include "rviz/ogre_helpers/point_cloud.h"
00059 # include "rviz/selection/forwards.h"
00060 #endif
00061 
00062 namespace rviz
00063 {
00064 class BoolProperty;
00065 class Display;
00066 class DisplayContext;
00067 class EnumProperty;
00068 class FloatProperty;
00069 struct IndexAndMessage;
00070 class PointCloudSelectionHandler;
00071 typedef boost::shared_ptr<PointCloudSelectionHandler> PointCloudSelectionHandlerPtr;
00072 class PointCloudTransformer;
00073 typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
00074 
00075 typedef std::vector<std::string> V_string;
00076 
00085 class PointCloudCommon: public QObject
00086 {
00087 Q_OBJECT
00088 public:
00089   struct CloudInfo
00090   {
00091     CloudInfo();
00092     ~CloudInfo();
00093 
00094     // clear the point cloud, but keep selection handler around
00095     void clear();
00096 
00097     ros::Time receive_time_;
00098 
00099     Ogre::SceneManager *manager_;
00100 
00101     sensor_msgs::PointCloud2ConstPtr message_;
00102 
00103     Ogre::SceneNode *scene_node_;
00104     boost::shared_ptr<PointCloud> cloud_;
00105     PointCloudSelectionHandlerPtr selection_handler_;
00106 
00107     std::vector<PointCloud::Point> transformed_points_;
00108 
00109     Ogre::Quaternion orientation_;
00110     Ogre::Vector3 position_;
00111 };
00112 
00113   typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
00114   typedef std::deque<CloudInfoPtr> D_CloudInfo;
00115   typedef std::vector<CloudInfoPtr> V_CloudInfo;
00116   typedef std::queue<CloudInfoPtr> Q_CloudInfo;
00117   typedef std::list<CloudInfoPtr> L_CloudInfo;
00118 
00119   PointCloudCommon( Display* display );
00120   ~PointCloudCommon();
00121 
00122   void initialize( DisplayContext* context, Ogre::SceneNode* scene_node );
00123 
00124   void fixedFrameChanged();
00125   void reset();
00126   void update(float wall_dt, float ros_dt);
00127 
00128   void addMessage(const sensor_msgs::PointCloudConstPtr& cloud);
00129   void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00130 
00131   ros::CallbackQueueInterface* getCallbackQueue() { return &cbqueue_; }
00132 
00133   Display* getDisplay() { return display_; }
00134 
00135   bool auto_size_;
00136 
00137   BoolProperty* selectable_property_;
00138   FloatProperty* point_world_size_property_;
00139   FloatProperty* point_pixel_size_property_;
00140   FloatProperty* alpha_property_;
00141   EnumProperty* xyz_transformer_property_;
00142   EnumProperty* color_transformer_property_;
00143   EnumProperty* style_property_;
00144   FloatProperty* decay_time_property_;
00145 
00146   void setAutoSize( bool auto_size );
00147 
00148 public Q_SLOTS:
00149   void causeRetransform();
00150 
00151 private Q_SLOTS:
00152   void updateSelectable();
00153   void updateStyle();
00154   void updateBillboardSize();
00155   void updateAlpha();
00156   void updateXyzTransformer();
00157   void updateColorTransformer();
00158   void setXyzTransformerOptions( EnumProperty* prop );
00159   void setColorTransformerOptions( EnumProperty* prop );
00160 
00161 private:
00162 
00166   bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
00167 
00168   void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00169   void updateStatus();
00170 
00171   PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00172   PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00173   void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
00174   void retransform();
00175   void onTransformerOptions(V_string& ops, uint32_t mask);
00176 
00177   void loadTransformers();
00178 
00179   float getSelectionBoxSize();
00180   void setPropertiesHidden( const QList<Property*>& props, bool hide );
00181   void fillTransformerOptions( EnumProperty* prop, uint32_t mask );
00182 
00183   ros::AsyncSpinner spinner_;
00184   ros::CallbackQueue cbqueue_;
00185 
00186   D_CloudInfo cloud_infos_;
00187 
00188   Ogre::SceneNode* scene_node_;
00189 
00190   V_CloudInfo new_cloud_infos_;
00191   boost::mutex new_clouds_mutex_;
00192 
00193   L_CloudInfo obsolete_cloud_infos_;
00194 
00195   struct TransformerInfo
00196   {
00197     PointCloudTransformerPtr transformer;
00198     QList<Property*> xyz_props;
00199     QList<Property*> color_props;
00200 
00201     std::string readable_name;
00202     std::string lookup_name;
00203   };
00204   typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
00205 
00206   boost::recursive_mutex transformers_mutex_;
00207   M_TransformerInfo transformers_;
00208   bool new_xyz_transformer_;
00209   bool new_color_transformer_;
00210   bool needs_retransform_;
00211 
00212   pluginlib::ClassLoader<PointCloudTransformer>* transformer_class_loader_;
00213 
00214   Display* display_;
00215   DisplayContext* context_;
00216 
00217   friend class PointCloudSelectionHandler;
00218 };
00219 
00220 class PointCloudSelectionHandler: public SelectionHandler
00221 {
00222 public:
00223   PointCloudSelectionHandler( float box_size, PointCloudCommon::CloudInfo* cloud_info, DisplayContext* context );
00224   virtual ~PointCloudSelectionHandler();
00225 
00226   virtual void createProperties( const Picked& obj, Property* parent_property );
00227   virtual void destroyProperties( const Picked& obj, Property* parent_property );
00228 
00229   virtual bool needsAdditionalRenderPass(uint32_t pass)
00230   {
00231     if (pass < 2)
00232     {
00233       return true;
00234     }
00235 
00236     return false;
00237   }
00238 
00239   virtual void preRenderPass(uint32_t pass);
00240   virtual void postRenderPass(uint32_t pass);
00241 
00242   virtual void onSelect(const Picked& obj);
00243   virtual void onDeselect(const Picked& obj);
00244 
00245   virtual void getAABBs(const Picked& obj, V_AABB& aabbs);
00246 
00247   void setBoxSize( float size ) { box_size_=size; }
00248 
00249   bool hasSelections() { return !boxes_.empty(); }
00250 
00251 private:
00252   PointCloudCommon::CloudInfo* cloud_info_;
00253   QHash<IndexAndMessage, Property*> property_hash_;
00254   float box_size_;
00255 };
00256 
00257 } // namespace rviz
00258 
00259 #endif // RVIZ_POINT_CLOUD_COMMON_H


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27