depth_cloud_display.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 // Forward declarations
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     // Obtain list of available topics
00109     RosTopicProperty::fillTopicList();
00110     // Apply filter
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   // Overrides from Display
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   // Property callbacks
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   // overrides from Display
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   // thread-safe status updates
00171   // add status update to global status list
00172   void updateStatus( StatusProperty::Level level, const QString& name, const QString& text );
00173 
00174   // use global status list to update rviz plugin status
00175   void setStatusList( );
00176 
00177   uint32_t messages_received_;
00178 
00179   boost::mutex mutex_;
00180 
00181   // ROS image subscription & synchronization
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   // RVIZ properties
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 } // namespace rviz
00225 
00226 #endif //RVIZ_DEPTHMAP_TO_POINTCLOUD_DISPLAY_H


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27