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_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
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 }
00263
00264 #endif // RVIZ_POINT_CLOUD_BASE_H