Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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 }
00258
00259 #endif // RVIZ_POINT_CLOUD_COMMON_H