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 protected Q_SLOTS:
00102   virtual void fillTopicList()
00103   {
00104     QStringList filtered_strings_;
00105 
00106     // Obtain list of available topics
00107     RosTopicProperty::fillTopicList();
00108     // Apply filter
00109     if (filter_enabled_)
00110       strings_ = strings_.filter(filter_);
00111   }
00112 private:
00113   QRegExp filter_;
00114   bool filter_enabled_;
00115 };
00116 
00121 class DepthCloudDisplay : public rviz::Display
00122 {
00123 Q_OBJECT
00124 public:
00125   DepthCloudDisplay();
00126   virtual ~DepthCloudDisplay();
00127 
00128   virtual void onInitialize();
00129 
00130   // Overrides from Display
00131   virtual void update(float wall_dt, float ros_dt);
00132   virtual void reset();
00133 
00134 protected Q_SLOTS:
00135   void updateQueueSize();
00137   void fillTransportOptionList(EnumProperty* property);
00138 
00139   // Property callbacks
00140   virtual void updateTopic();
00141   virtual void updateTopicFilter();
00142   virtual void updateUseAutoSize();
00143   virtual void updateAutoSizeFactor();
00144   virtual void updateUseOcclusionCompensation();
00145   virtual void updateOcclusionTimeOut();
00146 
00147 protected:
00148   void scanForTransportSubscriberPlugins();
00149 
00150   typedef std::vector<rviz::PointCloud::Point> V_Point;
00151 
00152   virtual void processMessage(sensor_msgs::Image::ConstPtr msg);
00153   virtual void processMessage(sensor_msgs::ImageConstPtr depth_msg, sensor_msgs::ImageConstPtr rgb_msg);
00154   void caminfoCallback( sensor_msgs::CameraInfo::ConstPtr msg );
00155 
00156   // overrides from Display
00157   virtual void onEnable();
00158   virtual void onDisable();
00159 
00160   virtual void fixedFrameChanged();
00161 
00162   void subscribe();
00163   void unsubscribe();
00164 
00165   void clear();
00166 
00167   // thread-safe status updates
00168   // add status update to global status list
00169   void updateStatus( StatusProperty::Level level, const QString& name, const QString& text );
00170 
00171   // use global status list to update rviz plugin status
00172   void setStatusList( );
00173 
00174   uint32_t messages_received_;
00175 
00176   boost::mutex mutex_;
00177 
00178   // ROS image subscription & synchronization
00179   image_transport::ImageTransport depthmap_it_;
00180   boost::shared_ptr<image_transport::SubscriberFilter > depthmap_sub_;
00181   boost::shared_ptr<tf::MessageFilter<sensor_msgs::Image> > depthmap_tf_filter_;
00182   image_transport::ImageTransport rgb_it_;
00183   boost::shared_ptr<image_transport::SubscriberFilter > rgb_sub_;
00184   boost::shared_ptr<message_filters::Subscriber<sensor_msgs::CameraInfo> > cam_info_sub_;
00185   sensor_msgs::CameraInfo::ConstPtr cam_info_;
00186   boost::mutex cam_info_mutex_;
00187 
00188   typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicyDepthColor;
00189   typedef message_filters::Synchronizer<SyncPolicyDepthColor> SynchronizerDepthColor;
00190 
00191   boost::shared_ptr<SynchronizerDepthColor> sync_depth_color_;
00192 
00193   // RVIZ properties
00194   Property* topic_filter_property_;
00195   IntProperty* queue_size_property_;
00196   BoolProperty* use_auto_size_property_;
00197   FloatProperty* auto_size_factor_property_;
00198   RosFilteredTopicProperty* depth_topic_property_;
00199   EnumProperty* depth_transport_property_;
00200   RosFilteredTopicProperty* color_topic_property_;
00201   EnumProperty* color_transport_property_;
00202   BoolProperty* use_occlusion_compensation_property_;
00203   FloatProperty* occlusion_shadow_timeout_property_;
00204 
00205   u_int32_t queue_size_;
00206 
00207   MultiLayerDepth* ml_depth_data_;
00208 
00209   Ogre::Quaternion current_orientation_;
00210   Ogre::Vector3 current_position_;
00211   float angular_thres_;
00212   float trans_thres_;
00213 
00214   PointCloudCommon* pointcloud_common_;
00215 
00216   std::set<std::string> transport_plugin_types_;
00217 
00218 };
00219 
00220 
00221 } // namespace rviz
00222 
00223 #endif //RVIZ_DEPTHMAP_TO_POINTCLOUD_DISPLAY_H


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:35