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 <deque>
00034 #include <queue>
00035 #include <vector>
00036
00037 #include <boost/shared_ptr.hpp>
00038 #include <boost/thread/mutex.hpp>
00039 #include <boost/thread/recursive_mutex.hpp>
00040
00041 #include <ros/spinner.h>
00042 #include <ros/callback_queue.h>
00043
00044 #include <message_filters/time_sequencer.h>
00045
00046 #include <pluginlib/class_loader.h>
00047
00048 #include <sensor_msgs/PointCloud.h>
00049 #include <sensor_msgs/PointCloud2.h>
00050
00051 #include "rviz/default_plugin/point_cloud_transformer.h"
00052 #include "rviz/display.h"
00053 #include "rviz/helpers/color.h"
00054 #include "rviz/ogre_helpers/point_cloud.h"
00055 #include "rviz/properties/bool_property.h"
00056 #include "rviz/properties/enum_property.h"
00057 #include "rviz/properties/float_property.h"
00058 #include "rviz/selection/forwards.h"
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 Q_OBJECT
00079 private:
00080 struct CloudInfo
00081 {
00082 CloudInfo();
00083 ~CloudInfo();
00084
00085 float time_;
00086
00087 Ogre::Matrix4 transform_;
00088 sensor_msgs::PointCloud2ConstPtr message_;
00089 uint32_t num_points_;
00090
00091 V_PointCloudPoint transformed_points_;
00092 };
00093 typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
00094 typedef std::deque<CloudInfoPtr> D_CloudInfo;
00095 typedef std::vector<CloudInfoPtr> V_CloudInfo;
00096 typedef std::queue<CloudInfoPtr> Q_CloudInfo;
00097
00098 public:
00103 enum Style
00104 {
00105 Points,
00106 Billboards,
00107 BillboardSpheres,
00108 Boxes,
00109
00110 StyleCount,
00111 };
00112
00117 enum ChannelRender
00118 {
00119 Intensity,
00120 Curvature,
00121 ColorRGBSpace,
00122 NormalSphere,
00123
00124 ChannelRenderCount,
00125 };
00126
00127 PointCloudBase();
00128 ~PointCloudBase();
00129
00130 void onInitialize();
00131
00132
00133 virtual void fixedFrameChanged();
00134 virtual void reset();
00135 virtual void update(float wall_dt, float ros_dt);
00136
00137 void causeRetransform();
00138
00139 protected:
00140 virtual void onEnable();
00141 virtual void onDisable();
00142
00143 private Q_SLOTS:
00144 void updateSelectable();
00145 void updateStyle();
00146 void updateBillboardSize();
00147 void updateAlpha();
00148 void updateXyzTransformer();
00149 void updateColorTransformer();
00150
00151 protected:
00152 typedef std::vector<PointCloud::Point> V_Point;
00153 typedef std::vector<V_Point> VV_Point;
00154
00158 bool transformCloud(const CloudInfoPtr& cloud, V_Point& points, bool fully_update_transformers);
00159
00160 void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00161 void addMessage(const sensor_msgs::PointCloudConstPtr& cloud);
00162 void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00163 void updateStatus();
00164
00165 PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00166 PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00167 void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
00168 void retransform();
00169 void onTransformerOptions(V_string& ops, uint32_t mask);
00170
00171 void loadTransformers();
00172
00173 ros::AsyncSpinner spinner_;
00174 ros::CallbackQueue cbqueue_;
00175
00176 D_CloudInfo clouds_;
00177 boost::mutex clouds_mutex_;
00178 bool new_cloud_;
00179
00180 PointCloud* cloud_;
00181 Ogre::SceneNode* scene_node_;
00182
00183 VV_Point new_points_;
00184 V_CloudInfo new_clouds_;
00185 boost::mutex new_clouds_mutex_;
00186
00187 struct TransformerInfo
00188 {
00189 PointCloudTransformerPtr transformer;
00190 QList<Property*> xyz_props;
00191 QList<Property*> color_props;
00192
00193 std::string readable_name;
00194 std::string lookup_name;
00195 };
00196 typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
00197
00198 boost::recursive_mutex transformers_mutex_;
00199 M_TransformerInfo transformers_;
00200 bool new_xyz_transformer_;
00201 bool new_color_transformer_;
00202 bool needs_retransform_;
00203
00204 CollObjectHandle coll_handle_;
00205 PointCloudSelectionHandlerPtr coll_handler_;
00206
00207 uint32_t messages_received_;
00208 uint32_t total_point_count_;
00209
00210 pluginlib::ClassLoader<PointCloudTransformer>* transformer_class_loader_;
00211
00212 BoolProperty* selectable_property_;
00213 FloatProperty* billboard_size_property_;
00214 FloatProperty* alpha_property_;
00215 EnumProperty* xyz_transformer_property_;
00216 EnumProperty* color_transformer_property_;
00217 EnumProperty* style_property_;
00218 FloatProperty* decay_time_property_;
00219
00220 friend class PointCloudSelectionHandler;
00221
00222 private:
00223 float getSelectionBoxSize();
00224 void setPropertiesHidden( const QList<Property*>& props, bool hide );
00225 };
00226
00227 }
00228
00229 #endif // RVIZ_POINT_CLOUD_BASE_H