point_cloud_base.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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_BASE_H
00031 #define RVIZ_POINT_CLOUD_BASE_H
00032 
00033 #include <pluginlib/class_loader.h>
00034 
00035 #include "point_cloud_transformer.h"
00036 
00037 #include "rviz/display.h"
00038 #include "rviz/helpers/color.h"
00039 #include "rviz/properties/forwards.h"
00040 #include "rviz/selection/forwards.h"
00041 
00042 #include "rviz/ogre_helpers/point_cloud.h"
00043 
00044 #include <message_filters/time_sequencer.h>
00045 
00046 #include "sensor_msgs/PointCloud.h"
00047 #include "sensor_msgs/PointCloud2.h"
00048 
00049 #include <ros/spinner.h>
00050 #include <ros/callback_queue.h>
00051 
00052 #include <boost/shared_ptr.hpp>
00053 #include <boost/thread/mutex.hpp>
00054 #include <boost/thread/recursive_mutex.hpp>
00055 
00056 #include <deque>
00057 #include <queue>
00058 #include <vector>
00059 
00060 namespace rviz
00061 {
00062 
00063 class PointCloudSelectionHandler;
00064 typedef boost::shared_ptr<PointCloudSelectionHandler> PointCloudSelectionHandlerPtr;
00065 class PointCloudTransformer;
00066 typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
00067 
00076 class PointCloudBase : public Display
00077 {
00078 private:
00079   struct CloudInfo
00080   {
00081     CloudInfo();
00082     ~CloudInfo();
00083 
00084     float time_;
00085 
00086     Ogre::Matrix4 transform_;
00087     sensor_msgs::PointCloud2ConstPtr message_;
00088     uint32_t num_points_;
00089 
00090     V_PointCloudPoint transformed_points_;
00091   };
00092   typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
00093   typedef std::deque<CloudInfoPtr> D_CloudInfo;
00094   typedef std::vector<CloudInfoPtr> V_CloudInfo;
00095   typedef std::queue<CloudInfoPtr> Q_CloudInfo;
00096 
00097 public:
00102   enum Style
00103   {
00104     Points,    
00105     Billboards,
00106     BillboardSpheres, 
00107     Boxes, 
00108 
00109     StyleCount,
00110   };
00111 
00116   enum ChannelRender
00117   {
00118     Intensity,    
00119     Curvature,    
00120     ColorRGBSpace,
00121     NormalSphere, 
00122 
00123     ChannelRenderCount,
00124   };
00125 
00126   PointCloudBase();
00127   ~PointCloudBase();
00128 
00129   void onInitialize();
00130 
00135   void setStyle( int style );
00141   void setBillboardSize( float size );
00146   void setDecayTime( float time );
00147 
00148   float getBillboardSize() { return billboard_size_; }
00149   int getStyle() { return style_; }
00150   float getDecayTime() { return point_decay_time_; }
00151 
00152   float getAlpha() { return alpha_; }
00153   void setAlpha( float alpha );
00154 
00155   bool getSelectable() { return selectable_; }
00156   void setSelectable(bool selectable);
00157 
00158   // Overrides from Display
00159   virtual void fixedFrameChanged();
00160   virtual void createProperties();
00161   virtual void reset();
00162   virtual void update(float wall_dt, float ros_dt);
00163 
00164   void causeRetransform();
00165 
00167   virtual void hideVisible();
00168 
00170   virtual void restoreVisible();
00171 
00172 protected:
00173   virtual void onEnable();
00174   virtual void onDisable();
00175 
00176   typedef std::vector<PointCloud::Point> V_Point;
00177   typedef std::vector<V_Point> VV_Point;
00178 
00182   bool transformCloud(const CloudInfoPtr& cloud, V_Point& points, bool fully_update_transformers);
00183 
00184   void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00185   void addMessage(const sensor_msgs::PointCloudConstPtr& cloud);
00186   void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00187   void updateStatus();
00188 
00189   void setXYZTransformer(const std::string& name);
00190   void setColorTransformer(const std::string& name);
00191   const std::string& getXYZTransformer() { return xyz_transformer_; }
00192   const std::string& getColorTransformer() { return color_transformer_; }
00193   PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00194   PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00195   void updateTransformers(const sensor_msgs::PointCloud2ConstPtr& cloud, bool fully_update);
00196   void retransform();
00197   void onTransformerOptions(V_string& ops, uint32_t mask);
00198 
00199   void loadTransformers();
00200 
00201   ros::AsyncSpinner spinner_;
00202   ros::CallbackQueue cbqueue_;
00203 
00204   D_CloudInfo clouds_;
00205   boost::mutex clouds_mutex_;
00206   bool new_cloud_;
00207 
00208   PointCloud* cloud_;
00209   Ogre::SceneNode* scene_node_;
00210 
00211   VV_Point new_points_;
00212   V_CloudInfo new_clouds_;
00213   boost::mutex new_clouds_mutex_;
00214 
00215   float alpha_;
00216 
00217   struct TransformerInfo
00218   {
00219     PointCloudTransformerPtr transformer;
00220     V_PropertyBaseWPtr xyz_props;
00221     V_PropertyBaseWPtr color_props;
00222 
00223     std::string readable_name;
00224     std::string lookup_name;
00225   };
00226   typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
00227 
00228   boost::recursive_mutex transformers_mutex_;
00229   M_TransformerInfo transformers_;
00230   std::string xyz_transformer_;
00231   std::string color_transformer_;
00232   bool new_xyz_transformer_;
00233   bool new_color_transformer_;
00234   bool needs_retransform_;
00235 
00236   int style_;                                 
00237   float billboard_size_;                      
00238   float point_decay_time_;                    
00239 
00240   bool selectable_;
00241   CollObjectHandle coll_handle_;
00242   PointCloudSelectionHandlerPtr coll_handler_;
00243 
00244   uint32_t messages_received_;
00245   uint32_t total_point_count_;
00246 
00247   pluginlib::ClassLoader<PointCloudTransformer>* transformer_class_loader_;
00248 
00249   BoolPropertyWPtr selectable_property_;
00250   FloatPropertyWPtr billboard_size_property_;
00251   FloatPropertyWPtr alpha_property_;
00252   EditEnumPropertyWPtr xyz_transformer_property_;
00253   EditEnumPropertyWPtr color_transformer_property_;
00254   EnumPropertyWPtr style_property_;
00255   FloatPropertyWPtr decay_time_property_;
00256 
00257   bool hidden_;
00258 
00259   friend class PointCloudSelectionHandler;
00260 };
00261 
00262 } // namespace rviz
00263 
00264 #endif // RVIZ_POINT_CLOUD_BASE_H


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32