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