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_DEPTH_CLOUD_DISPLAY_H
00031 #define RVIZ_DEPTH_CLOUD_DISPLAY_H
00032
00033 #ifndef Q_MOC_RUN
00034 # include <boost/shared_ptr.hpp>
00035 # include <boost/thread/mutex.hpp>
00036
00037 # include <ros/ros.h>
00038 # include <image_transport/image_transport.h>
00039 # include <image_transport/subscriber_filter.h>
00040 # include <message_filters/subscriber.h>
00041 # include <message_filters/synchronizer.h>
00042 # include <message_filters/sync_policies/approximate_time.h>
00043 # include <tf/message_filter.h>
00044
00045 # include "rviz/properties/enum_property.h"
00046 # include "rviz/properties/float_property.h"
00047 # include "rviz/properties/bool_property.h"
00048 # include "rviz/properties/int_property.h"
00049 # include "rviz/properties/ros_topic_property.h"
00050
00051 # include <rviz/display.h>
00052
00053 # include <rviz/default_plugin/point_cloud_common.h>
00054 #endif
00055
00056 #include <QMap>
00057
00058 using namespace message_filters::sync_policies;
00059
00060
00061
00062 namespace image_transport
00063 {
00064 class SubscriberFilter;
00065 }
00066
00067 namespace rviz
00068 {
00069
00070 class EnumProperty;
00071 class FloatProperty;
00072 class BoolProperty;
00073 class IntProperty;
00074
00075 class MultiLayerDepth;
00076
00077 class RosFilteredTopicProperty: public RosTopicProperty
00078 {
00079 Q_OBJECT
00080 public:
00081 RosFilteredTopicProperty( const QString& name = QString(),
00082 const QString& default_value = QString(),
00083 const QString& message_type = QString(),
00084 const QString& description = QString(),
00085 const QRegExp& filter = QRegExp(),
00086 Property* parent = 0,
00087 const char *changed_slot = 0,
00088 QObject* receiver = 0) :
00089 RosTopicProperty(name, default_value, message_type, description, parent, changed_slot, receiver), filter_(filter), filter_enabled_(true)
00090 {
00091
00092 }
00093
00094 public:
00095 void enableFilter (bool enabled)
00096 {
00097 filter_enabled_ = enabled;
00098 fillTopicList();
00099 }
00100
00101 QRegExp filter() const { return filter_; }
00102
00103 protected Q_SLOTS:
00104 virtual void fillTopicList()
00105 {
00106 QStringList filtered_strings_;
00107
00108
00109 RosTopicProperty::fillTopicList();
00110
00111 if (filter_enabled_)
00112 strings_ = strings_.filter(filter_);
00113 }
00114 private:
00115 QRegExp filter_;
00116 bool filter_enabled_;
00117 };
00118
00123 class DepthCloudDisplay : public rviz::Display
00124 {
00125 Q_OBJECT
00126 public:
00127 DepthCloudDisplay();
00128 virtual ~DepthCloudDisplay();
00129
00130 virtual void onInitialize();
00131
00132
00133 virtual void update(float wall_dt, float ros_dt);
00134 virtual void reset();
00135 virtual void setTopic( const QString &topic, const QString &datatype );
00136
00137 protected Q_SLOTS:
00138 void updateQueueSize();
00140 void fillTransportOptionList(EnumProperty* property);
00141
00142
00143 virtual void updateTopic();
00144 virtual void updateTopicFilter();
00145 virtual void updateUseAutoSize();
00146 virtual void updateAutoSizeFactor();
00147 virtual void updateUseOcclusionCompensation();
00148 virtual void updateOcclusionTimeOut();
00149
00150 protected:
00151 void scanForTransportSubscriberPlugins();
00152
00153 typedef std::vector<rviz::PointCloud::Point> V_Point;
00154
00155 virtual void processMessage(sensor_msgs::Image::ConstPtr msg);
00156 virtual void processMessage(sensor_msgs::ImageConstPtr depth_msg, sensor_msgs::ImageConstPtr rgb_msg);
00157 void caminfoCallback( sensor_msgs::CameraInfo::ConstPtr msg );
00158
00159
00160 virtual void onEnable();
00161 virtual void onDisable();
00162
00163 virtual void fixedFrameChanged();
00164
00165 void subscribe();
00166 void unsubscribe();
00167
00168 void clear();
00169
00170
00171
00172 void updateStatus( StatusProperty::Level level, const QString& name, const QString& text );
00173
00174
00175 void setStatusList( );
00176
00177 uint32_t messages_received_;
00178
00179 boost::mutex mutex_;
00180
00181
00182 boost::scoped_ptr<image_transport::ImageTransport> depthmap_it_;
00183 boost::shared_ptr<image_transport::SubscriberFilter > depthmap_sub_;
00184 boost::shared_ptr<tf::MessageFilter<sensor_msgs::Image> > depthmap_tf_filter_;
00185 boost::scoped_ptr<image_transport::ImageTransport> rgb_it_;
00186 boost::shared_ptr<image_transport::SubscriberFilter > rgb_sub_;
00187 boost::shared_ptr<message_filters::Subscriber<sensor_msgs::CameraInfo> > cam_info_sub_;
00188 sensor_msgs::CameraInfo::ConstPtr cam_info_;
00189 boost::mutex cam_info_mutex_;
00190
00191 typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicyDepthColor;
00192 typedef message_filters::Synchronizer<SyncPolicyDepthColor> SynchronizerDepthColor;
00193
00194 boost::shared_ptr<SynchronizerDepthColor> sync_depth_color_;
00195
00196
00197 Property* topic_filter_property_;
00198 IntProperty* queue_size_property_;
00199 BoolProperty* use_auto_size_property_;
00200 FloatProperty* auto_size_factor_property_;
00201 RosFilteredTopicProperty* depth_topic_property_;
00202 EnumProperty* depth_transport_property_;
00203 RosFilteredTopicProperty* color_topic_property_;
00204 EnumProperty* color_transport_property_;
00205 BoolProperty* use_occlusion_compensation_property_;
00206 FloatProperty* occlusion_shadow_timeout_property_;
00207
00208 u_int32_t queue_size_;
00209
00210 MultiLayerDepth* ml_depth_data_;
00211
00212 Ogre::Quaternion current_orientation_;
00213 Ogre::Vector3 current_position_;
00214 float angular_thres_;
00215 float trans_thres_;
00216
00217 PointCloudCommon* pointcloud_common_;
00218
00219 std::set<std::string> transport_plugin_types_;
00220
00221 };
00222
00223
00224 }
00225
00226 #endif //RVIZ_DEPTHMAP_TO_POINTCLOUD_DISPLAY_H