00001 // -*- mode: C++ -*- 00002 #ifndef NORMAL_DISPLAY_H 00003 #define NORMAL_DISPLAY_H 00004 #ifndef Q_MOC_RUN 00005 #include <OGRE/OgreSceneNode.h> 00006 #include <OGRE/OgreSceneManager.h> 00007 #include <QColor> 00008 00009 #include <boost/circular_buffer.hpp> 00010 00011 #include <sensor_msgs/PointCloud2.h> 00012 00013 #include <rviz/message_filter_display.h> 00014 #include <rviz/default_plugin/point_cloud_transformers.h> 00015 #include <rviz/validate_floats.h> 00016 #include <rviz/visualization_manager.h> 00017 #include <rviz/frame_manager.h> 00018 #include <rviz/properties/enum_property.h> 00019 #include <rviz/properties/color_property.h> 00020 #include <rviz/properties/int_property.h> 00021 #include <rviz/properties/bool_property.h> 00022 #include <rviz/properties/float_property.h> 00023 #include "normal_visual.h" 00024 #endif 00025 00026 namespace jsk_rviz_plugins 00027 { 00028 00029 class NormalDisplay: public rviz::MessageFilterDisplay<sensor_msgs::PointCloud2> 00030 { 00031 Q_OBJECT 00032 public: 00033 NormalDisplay(); 00034 virtual ~NormalDisplay(); 00035 rviz::EnumProperty* style_property_; 00036 rviz::ColorProperty* color_property_; 00037 rviz::ColorProperty* min_color_property_; 00038 rviz::ColorProperty* max_color_property_; 00039 rviz::FloatProperty* skip_rate_property_; 00040 rviz::BoolProperty* rainbow_property_; 00041 rviz::FloatProperty* scale_property_; 00042 rviz::FloatProperty* alpha_property_; 00043 float skip_rate_; 00044 float scale_; 00045 float alpha_; 00046 00047 enum ColorTypes{ 00048 POINTS_COLOR, 00049 FLAT_COLOR, 00050 DIRECTION_COLOR, 00051 CURVATURE_COLOR 00052 }; 00053 00054 protected: 00055 virtual void onInitialize(); 00056 00057 virtual void reset(); 00058 00059 boost::circular_buffer<std::shared_ptr<NormalVisual> > visuals_; 00060 00061 // Function to handle an incoming ROS message. 00062 private Q_SLOTS: 00063 void processMessage( const sensor_msgs::PointCloud2::ConstPtr& msg ); 00064 void updateStyle(); 00065 void updateSkipRate(); 00066 void updateRainbow(); 00067 void updateScale(); 00068 void updateAlpha(); 00069 void getRainbow(float value , float& rf, float& gf, float& bf); 00070 }; 00071 00072 } 00073 00074 #endif