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 "point_cloud_transformer.h"
00034
00035 #include "rviz/display.h"
00036 #include "rviz/helpers/color.h"
00037 #include "rviz/properties/forwards.h"
00038 #include "rviz/selection/forwards.h"
00039
00040 #include "ogre_tools/point_cloud.h"
00041
00042 #include <message_filters/time_sequencer.h>
00043
00044 #include "sensor_msgs/PointCloud.h"
00045 #include "sensor_msgs/PointCloud2.h"
00046
00047 #include <ros/spinner.h>
00048 #include <ros/callback_queue.h>
00049
00050 #include <boost/shared_ptr.hpp>
00051 #include <boost/thread.hpp>
00052 #include <boost/signals/connection.hpp>
00053 #include <boost/signals/trackable.hpp>
00054
00055 #include <deque>
00056 #include <queue>
00057 #include <vector>
00058
00059 namespace rviz
00060 {
00061
00062 class Plugin;
00063 typedef boost::shared_ptr<Plugin> PluginPtr;
00064 class PluginStatus;
00065
00066 class PointCloudSelectionHandler;
00067 typedef boost::shared_ptr<PointCloudSelectionHandler> PointCloudSelectionHandlerPtr;
00068 class PointCloudTransformer;
00069 typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
00070
00079 class PointCloudBase : public Display, public boost::signals::trackable
00080 {
00081 private:
00082 struct CloudInfo
00083 {
00084 CloudInfo();
00085 ~CloudInfo();
00086
00087 float time_;
00088
00089 Ogre::Matrix4 transform_;
00090 sensor_msgs::PointCloud2ConstPtr message_;
00091 uint32_t num_points_;
00092
00093 PointCloud transformed_points_;
00094 };
00095 typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
00096 typedef std::deque<CloudInfoPtr> D_CloudInfo;
00097 typedef std::vector<CloudInfoPtr> V_CloudInfo;
00098 typedef std::queue<CloudInfoPtr> Q_CloudInfo;
00099
00100 public:
00105 enum Style
00106 {
00107 Points,
00108 Billboards,
00109 BillboardSpheres,
00110 Boxes,
00111
00112 StyleCount,
00113 };
00114
00119 enum ChannelRender
00120 {
00121 Intensity,
00122 Curvature,
00123 ColorRGBSpace,
00124 NormalSphere,
00125
00126 ChannelRenderCount,
00127 };
00128
00129 PointCloudBase( const std::string& name, VisualizationManager* manager );
00130 ~PointCloudBase();
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
00166 protected:
00167 virtual void onEnable();
00168 virtual void onDisable();
00169
00170 typedef std::vector<ogre_tools::PointCloud::Point> V_Point;
00171 typedef std::vector<V_Point> VV_Point;
00172
00176 bool transformCloud(const CloudInfoPtr& cloud, V_Point& points, bool fully_update_transformers);
00177
00178 void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00179 void addMessage(const sensor_msgs::PointCloudConstPtr& cloud);
00180 void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
00181 void updateStatus();
00182
00183 void setXYZTransformer(const std::string& name);
00184 void setColorTransformer(const std::string& name);
00185 const std::string& getXYZTransformer() { return xyz_transformer_; }
00186 const std::string& getColorTransformer() { return color_transformer_; }
00187 PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00188 PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
00189 void updateTransformers(const sensor_msgs::PointCloud2ConstPtr& cloud, bool fully_update);
00190 void retransform();
00191 void onTransformerOptions(V_string& ops, uint32_t mask);
00192
00193 void onPluginLoaded(const PluginStatus& status);
00194 void onPluginUnloading(const PluginStatus& status);
00195 void loadTransformers(Plugin* plugin);
00196
00197 ros::AsyncSpinner spinner_;
00198 ros::CallbackQueue cbqueue_;
00199
00200 D_CloudInfo clouds_;
00201 boost::mutex clouds_mutex_;
00202 bool new_cloud_;
00203
00204 ogre_tools::PointCloud* cloud_;
00205 Ogre::SceneNode* scene_node_;
00206
00207 VV_Point new_points_;
00208 V_CloudInfo new_clouds_;
00209 boost::mutex new_clouds_mutex_;
00210
00211 float alpha_;
00212
00213 struct TransformerInfo
00214 {
00215 PointCloudTransformerPtr transformer;
00216 V_PropertyBaseWPtr xyz_props;
00217 V_PropertyBaseWPtr color_props;
00218
00219 std::string readable_name;
00220 Plugin* plugin;
00221 };
00222 typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
00223
00224 boost::recursive_mutex transformers_mutex_;
00225 M_TransformerInfo transformers_;
00226 std::string xyz_transformer_;
00227 std::string color_transformer_;
00228 bool new_xyz_transformer_;
00229 bool new_color_transformer_;
00230 bool needs_retransform_;
00231
00232 int style_;
00233 float billboard_size_;
00234 float point_decay_time_;
00235
00236 bool selectable_;
00237 CollObjectHandle coll_handle_;
00238 PointCloudSelectionHandlerPtr coll_handler_;
00239
00240 uint32_t messages_received_;
00241 uint32_t total_point_count_;
00242
00243 BoolPropertyWPtr selectable_property_;
00244 FloatPropertyWPtr billboard_size_property_;
00245 FloatPropertyWPtr alpha_property_;
00246 EditEnumPropertyWPtr xyz_transformer_property_;
00247 EditEnumPropertyWPtr color_transformer_property_;
00248 EnumPropertyWPtr style_property_;
00249 FloatPropertyWPtr decay_time_property_;
00250
00251 struct PluginConns
00252 {
00253 boost::signals::connection loaded;
00254 boost::signals::connection unloading;
00255 };
00256 typedef std::map<Plugin*, PluginConns> M_PluginConns;
00257 M_PluginConns plugin_conns_;
00258
00259 friend class PointCloudSelectionHandler;
00260 };
00261
00262 }
00263
00264 #endif // RVIZ_POINT_CLOUD_BASE_H