depth_cloud_display.cpp
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 #include <QObject>
00030 
00031 #include "depth_cloud_display.h"
00032 #include "rviz/visualization_manager.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/validate_floats.h"
00035 
00036 #include "rviz/properties/enum_property.h"
00037 #include "rviz/properties/float_property.h"
00038 #include "rviz/properties/bool_property.h"
00039 #include "rviz/properties/int_property.h"
00040 #include "rviz/frame_manager.h"
00041 
00042 #include <tf/transform_listener.h>
00043 
00044 #include <boost/bind.hpp>
00045 #include <boost/algorithm/string/erase.hpp>
00046 #include <boost/foreach.hpp>
00047 #include <boost/shared_ptr.hpp>
00048 
00049 #include <OGRE/OgreSceneNode.h>
00050 #include <OGRE/OgreSceneManager.h>
00051 
00052 #include <image_transport/camera_common.h>
00053 #include <image_transport/subscriber_plugin.h>
00054 #include <image_transport/subscriber_filter.h>
00055 
00056 #include <sensor_msgs/image_encodings.h>
00057 
00058 #include "depth_cloud_mld.h"
00059 
00060 #include <sstream>
00061 #include <string>
00062 #include <math.h>
00063 
00064 namespace enc = sensor_msgs::image_encodings;
00065 
00066 namespace rviz
00067 {
00068 
00069 DepthCloudDisplay::DepthCloudDisplay()
00070   : rviz::Display()
00071   , messages_received_(0)
00072   , depthmap_it_(threaded_nh_)
00073   , depthmap_sub_()
00074   , rgb_it_ (threaded_nh_)
00075   , rgb_sub_()
00076   , cam_info_sub_()
00077   , queue_size_(5)
00078   , ml_depth_data_(new MultiLayerDepth())
00079   , angular_thres_(0.5f)
00080   , trans_thres_(0.01f)
00081 
00082 {
00083 
00084   // Depth map properties
00085   QRegExp depth_filter("depth");
00086   depth_filter.setCaseSensitivity(Qt::CaseInsensitive);
00087 
00088   topic_filter_property_ = new Property("Topic Filter",
00089                                         true,
00090                                         "List only topics with names that relate to depth and color images",
00091                                         this,
00092                                         SLOT (updateTopicFilter() ));
00093 
00094   depth_topic_property_ = new RosFilteredTopicProperty("Depth Map Topic",
00095                                                        "",
00096                                                        QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
00097                                                        "sensor_msgs::Image topic to subscribe to.",
00098                                                        depth_filter,
00099                                                        this,
00100                                                        SLOT( updateTopic() ));
00101 
00102   depth_transport_property_ = new EnumProperty("Depth Map Transport Hint", "raw", "Preferred method of sending images.", this, SLOT( updateTopic() ));
00103 
00104   connect(depth_transport_property_, SIGNAL( requestOptions( EnumProperty* )), this,  SLOT( fillTransportOptionList( EnumProperty* )));
00105 
00106   depth_transport_property_->setStdString("raw");
00107 
00108   // color image properties
00109   QRegExp color_filter("color|rgb|bgr|gray|mono");
00110   color_filter.setCaseSensitivity(Qt::CaseInsensitive);
00111 
00112   color_topic_property_ = new RosFilteredTopicProperty("Color Image Topic",
00113                                                        "",
00114                                                        QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
00115                                                        "sensor_msgs::Image topic to subscribe to.",
00116                                                        color_filter,
00117                                                        this,
00118                                                        SLOT( updateTopic() ));
00119 
00120   color_transport_property_ = new EnumProperty("Color Transport Hint", "raw", "Preferred method of sending images.", this, SLOT( updateTopic() ));
00121 
00122 
00123   connect(color_transport_property_, SIGNAL( requestOptions( EnumProperty* )), this, SLOT( fillTransportOptionList( EnumProperty* )));
00124 
00125   color_transport_property_->setStdString("raw");
00126 
00127   // Queue size property
00128   queue_size_property_ = new IntProperty( "Queue Size", queue_size_,
00129                                           "Advanced: set the size of the incoming message queue.  Increasing this "
00130                                           "is useful if your incoming TF data is delayed significantly from your"
00131                                           " image data, but it can greatly increase memory usage if the messages are big.",
00132                                           this,
00133                                           SLOT( updateQueueSize() ));
00134   queue_size_property_->setMin( 1 );
00135 
00136   use_auto_size_property_ = new BoolProperty( "Auto Size",
00137                                               true,
00138                                               "Automatically scale each point based on its depth value and the camera parameters.",
00139                                               this,
00140                                               SLOT( updateUseAutoSize() ),
00141                                               this );
00142 
00143   auto_size_factor_property_ = new FloatProperty( "Auto Size Factor", 1,
00144                                                   "Scaling factor to be applied to the auto size.",
00145                                                   use_auto_size_property_,
00146                                                   SLOT( updateAutoSizeFactor() ),
00147                                                   this );
00148   auto_size_factor_property_->setMin( 0.0001 );
00149 
00150   use_occlusion_compensation_property_ = new BoolProperty( "Occlusion Compensation",
00151                                                            false,
00152                                                            "Keep points alive after they have been occluded by a closer point. Points are removed after a timeout or when the camera frame moves.",
00153                                                            this,
00154                                                            SLOT( updateUseOcclusionCompensation() ),
00155                                                            this );
00156 
00157   occlusion_shadow_timeout_property_ = new FloatProperty( "Occlusion Time-Out",
00158                                                           30.0f,
00159                                                           "Amount of seconds before removing occluded points from the depth cloud",
00160                                                           use_occlusion_compensation_property_,
00161                                                           SLOT( updateOcclusionTimeOut() ),
00162                                                           this );
00163 
00164   // Instantiate PointCloudCommon class for displaying point clouds
00165   pointcloud_common_ = new PointCloudCommon(this);
00166 
00167   updateUseAutoSize();
00168   updateUseOcclusionCompensation();
00169 
00170   // PointCloudCommon sets up a callback queue with a thread for each
00171   // instance.  Use that for processing incoming messages.
00172   threaded_nh_.setCallbackQueue( pointcloud_common_->getCallbackQueue() );
00173 
00174   // Scan for available transport plugins
00175   scanForTransportSubscriberPlugins();
00176 }
00177 
00178 void DepthCloudDisplay::onInitialize()
00179 {
00180   pointcloud_common_->initialize(context_, scene_node_);
00181   pointcloud_common_->xyz_transformer_property_->hide();
00182 }
00183 
00184 DepthCloudDisplay::~DepthCloudDisplay()
00185 {
00186 
00187   unsubscribe();
00188 
00189   if (pointcloud_common_)
00190     delete pointcloud_common_;
00191 
00192   if (ml_depth_data_)
00193         delete ml_depth_data_;
00194 }
00195 
00196 void DepthCloudDisplay::updateQueueSize()
00197 {
00198   queue_size_ = queue_size_property_->getInt();
00199 }
00200 
00201 void DepthCloudDisplay::updateUseAutoSize()
00202 {
00203   bool use_auto_size = use_auto_size_property_->getBool();
00204   pointcloud_common_->point_world_size_property_->setReadOnly( use_auto_size );
00205   pointcloud_common_->setAutoSize( use_auto_size );
00206   auto_size_factor_property_->setHidden(!use_auto_size);
00207   if (use_auto_size)
00208           use_auto_size_property_->expand();
00209 }
00210 
00211 void DepthCloudDisplay::updateAutoSizeFactor()
00212 {
00213 }
00214 
00215 void DepthCloudDisplay::updateTopicFilter()
00216 {
00217   bool enabled = topic_filter_property_->getValue().toBool();
00218   depth_topic_property_->enableFilter(enabled);
00219   color_topic_property_->enableFilter(enabled);
00220 }
00221 
00222 void DepthCloudDisplay::updateUseOcclusionCompensation()
00223 {
00224   bool use_occlusion_compensation = use_occlusion_compensation_property_->getBool();
00225   occlusion_shadow_timeout_property_->setHidden(!use_occlusion_compensation);
00226 
00227   if (use_occlusion_compensation)
00228   {
00229     updateOcclusionTimeOut();
00230     ml_depth_data_->enableOcclusionCompensation(true);
00231     use_occlusion_compensation_property_->expand();
00232   } else
00233   {
00234     ml_depth_data_->enableOcclusionCompensation(false);
00235   }
00236 }
00237 
00238 void DepthCloudDisplay::updateOcclusionTimeOut()
00239 {
00240   float occlusion_timeout = occlusion_shadow_timeout_property_->getFloat();
00241   ml_depth_data_->setShadowTimeOut(occlusion_timeout);
00242 }
00243 
00244 void DepthCloudDisplay::onEnable()
00245 {
00246   subscribe();
00247 }
00248 
00249 void DepthCloudDisplay::onDisable()
00250 {
00251   unsubscribe();
00252 
00253   ml_depth_data_->reset();
00254 
00255   clear();
00256 }
00257 
00258 void DepthCloudDisplay::subscribe()
00259 {
00260   if ( !isEnabled() )
00261   {
00262     return;
00263   }
00264 
00265   try
00266   {
00267     // reset all message filters
00268     sync_depth_color_.reset(new SynchronizerDepthColor(SyncPolicyDepthColor(queue_size_)));
00269     depthmap_tf_filter_.reset();
00270     depthmap_sub_.reset(new image_transport::SubscriberFilter());
00271     rgb_sub_.reset(new image_transport::SubscriberFilter());
00272     cam_info_sub_.reset(new message_filters::Subscriber<sensor_msgs::CameraInfo>());
00273 
00274     std::string depthmap_topic = depth_topic_property_->getTopicStd();
00275     std::string color_topic = color_topic_property_->getTopicStd();
00276 
00277     std::string depthmap_transport = depth_transport_property_->getStdString();
00278     std::string color_transport = color_transport_property_->getStdString();
00279 
00280     if (!depthmap_topic.empty() && !depthmap_transport.empty()) {
00281       // subscribe to depth map topic
00282       depthmap_sub_->subscribe(depthmap_it_, depthmap_topic, queue_size_,  image_transport::TransportHints(depthmap_transport));
00283 
00284       depthmap_tf_filter_.reset(
00285           new tf::MessageFilter<sensor_msgs::Image>(*depthmap_sub_, *context_->getTFClient(), fixed_frame_.toStdString(), queue_size_, threaded_nh_));
00286 
00287       // subscribe to CameraInfo  topic
00288       std::string info_topic = image_transport::getCameraInfoTopic(depthmap_topic);
00289       cam_info_sub_->subscribe(threaded_nh_, info_topic, queue_size_);
00290       cam_info_sub_->registerCallback(boost::bind(&DepthCloudDisplay::caminfoCallback, this, _1));
00291 
00292       if (!color_topic.empty() && !color_transport.empty()) {
00293         // subscribe to color image topic
00294         rgb_sub_->subscribe(rgb_it_, color_topic, queue_size_,  image_transport::TransportHints(color_transport));
00295 
00296         // connect message filters to synchronizer
00297         sync_depth_color_->connectInput(*depthmap_tf_filter_, *rgb_sub_);
00298         sync_depth_color_->setInterMessageLowerBound(0, ros::Duration(0.5));
00299         sync_depth_color_->setInterMessageLowerBound(1, ros::Duration(0.5));
00300         sync_depth_color_->registerCallback(boost::bind(&DepthCloudDisplay::processMessage, this, _1, _2));
00301 
00302         pointcloud_common_->color_transformer_property_->setValue("RGB8");
00303       } else
00304       {
00305         depthmap_tf_filter_->registerCallback(boost::bind(&DepthCloudDisplay::processMessage, this, _1));
00306       }
00307 
00308     }
00309   }
00310   catch (ros::Exception& e)
00311   {
00312     setStatus( StatusProperty::Error, "Message", QString("Error subscribing: ") + e.what() );
00313   }
00314   catch (image_transport::TransportLoadException& e)
00315   {
00316     setStatus( StatusProperty::Error, "Message", QString("Error subscribing: ") + e.what() );
00317   }
00318 }
00319 
00320 void DepthCloudDisplay::caminfoCallback( sensor_msgs::CameraInfo::ConstPtr msg )
00321 {
00322   boost::mutex::scoped_lock lock(cam_info_mutex_);
00323   cam_info_ = msg;
00324  }
00325 
00326 void DepthCloudDisplay::unsubscribe()
00327 {
00328   clear();
00329 
00330   try
00331   {
00332     // reset all filters
00333     sync_depth_color_.reset(new SynchronizerDepthColor(SyncPolicyDepthColor(queue_size_)));
00334     depthmap_tf_filter_.reset();
00335     depthmap_sub_.reset();
00336     rgb_sub_.reset();
00337     cam_info_sub_.reset();
00338   }
00339   catch (ros::Exception& e)
00340   {
00341     setStatus( StatusProperty::Error, "Message", QString("Error unsubscribing: ") + e.what() );
00342   }
00343 }
00344 
00345 
00346 void DepthCloudDisplay::clear()
00347 {
00348 
00349   boost::mutex::scoped_lock lock(mutex_);
00350 
00351   pointcloud_common_->reset();
00352 }
00353 
00354 
00355 void DepthCloudDisplay::update(float wall_dt, float ros_dt)
00356 {
00357 
00358   boost::mutex::scoped_lock lock(mutex_);
00359 
00360   pointcloud_common_->update(wall_dt, ros_dt);
00361 }
00362 
00363 
00364 void DepthCloudDisplay::reset()
00365 {
00366   clear();
00367   messages_received_ = 0;
00368   setStatus( StatusProperty::Ok, "Depth Map", "0 depth maps received" );
00369   setStatus( StatusProperty::Ok, "Message", "Ok" );
00370 }
00371 
00372 void DepthCloudDisplay::processMessage(sensor_msgs::ImageConstPtr depth_msg)
00373 {
00374   processMessage(depth_msg, sensor_msgs::ImageConstPtr());
00375 }
00376 
00377 void DepthCloudDisplay::processMessage(sensor_msgs::ImageConstPtr depth_msg,
00378                                        sensor_msgs::ImageConstPtr rgb_msg)
00379 {
00380   if (context_->getFrameManager()->getPause() )
00381   {
00382     return;
00383   }
00384 
00385   std::ostringstream s;
00386 
00387   ++messages_received_;
00388   setStatus( StatusProperty::Ok, "Depth Map", QString::number(messages_received_) + " depth maps received");
00389   setStatus( StatusProperty::Ok, "Message", "Ok" );
00390 
00391   sensor_msgs::CameraInfo::ConstPtr cam_info;
00392   {
00393     boost::mutex::scoped_lock lock(cam_info_mutex_);
00394     cam_info = cam_info_;
00395   }
00396 
00397   if ( !cam_info || !depth_msg )
00398   {
00399     return;
00400   }
00401 
00402   s.str("");
00403   s << depth_msg->width << " x " << depth_msg->height;
00404   setStatusStd( StatusProperty::Ok, "Depth Image Size", s.str() );
00405 
00406   if (rgb_msg)
00407   {
00408     s.str("");
00409     s << rgb_msg->width << " x " << rgb_msg->height;
00410     setStatusStd( StatusProperty::Ok, "Image Size", s.str() );
00411 
00412     if (depth_msg->header.frame_id != rgb_msg->header.frame_id)
00413     {
00414       std::stringstream errorMsg;
00415       errorMsg << "Depth image frame id [" << depth_msg->header.frame_id.c_str()
00416            << "] doesn't match color image frame id [" << rgb_msg->header.frame_id.c_str() << "]";
00417       setStatusStd( StatusProperty::Warn, "Message", errorMsg.str() );
00418     }
00419   }
00420 
00421   if ( use_auto_size_property_->getBool() )
00422   {
00423     float f = cam_info->K[0];
00424     float bx = cam_info->binning_x > 0 ? cam_info->binning_x : 1.0;
00425     float s = auto_size_factor_property_->getFloat();
00426     pointcloud_common_->point_world_size_property_->setFloat( s / f * bx );
00427   }
00428 
00429   bool use_occlusion_compensation = use_occlusion_compensation_property_->getBool();
00430 
00431   if (use_occlusion_compensation)
00432   {
00433     // reset depth cloud display if camera moves
00434     Ogre::Quaternion orientation;
00435     Ogre::Vector3 position;
00436 
00437     if (!context_->getFrameManager()->getTransform(depth_msg->header, position, orientation))
00438     {
00439       setStatus(
00440           StatusProperty::Error,
00441           "Message",
00442           QString("Failed to transform from frame [") + depth_msg->header.frame_id.c_str() + QString("] to frame [")
00443               + context_->getFrameManager()->getFixedFrame().c_str() + QString("]"));
00444       return;
00445     }
00446     else
00447     {
00448       Ogre::Radian angle;
00449       Ogre::Vector3 axis;
00450 
00451       (current_orientation_.Inverse() * orientation).ToAngleAxis(angle, axis);
00452 
00453       float angle_deg = angle.valueDegrees();
00454       if (angle_deg >= 180.0f)
00455         angle_deg -= 180.0f;
00456       if (angle_deg < -180.0f)
00457         angle_deg += 180.0f;
00458 
00459       if (trans_thres_ == 0.0 || angular_thres_ == 0.0 ||
00460           (position - current_position_).length() > trans_thres_
00461           || angle_deg > angular_thres_)
00462       {
00463         // camera orientation/position changed
00464         current_position_ = position;
00465         current_orientation_ = orientation;
00466 
00467         // reset multi-layered depth image
00468         ml_depth_data_->reset();
00469       }
00470     }
00471   }
00472 
00473   try
00474   {
00475     sensor_msgs::PointCloud2Ptr cloud_msg = ml_depth_data_->generatePointCloudFromDepth(depth_msg, rgb_msg, cam_info);
00476 
00477     if ( !cloud_msg.get() )
00478     {
00479       throw MultiLayerDepthException("generatePointCloudFromDepth() returned zero.");
00480     }
00481     cloud_msg->header = depth_msg->header;
00482 
00483     // add point cloud message to pointcloud_common to be visualized
00484     pointcloud_common_->addMessage(cloud_msg);
00485   }
00486   catch (MultiLayerDepthException& e)
00487   {
00488     setStatus(StatusProperty::Error, "Message", QString("Error updating depth cloud: ") + e.what());
00489   }
00490 
00491 
00492 
00493 }
00494 
00495 
00496 void DepthCloudDisplay::scanForTransportSubscriberPlugins()
00497 {
00498   pluginlib::ClassLoader<image_transport::SubscriberPlugin> sub_loader("image_transport",
00499                                                                        "image_transport::SubscriberPlugin");
00500 
00501   BOOST_FOREACH( const std::string& lookup_name, sub_loader.getDeclaredClasses() )
00502   {
00503     // lookup_name is formatted as "pkg/transport_sub", for instance
00504     // "image_transport/compressed_sub" for the "compressed"
00505     // transport.  This code removes the "_sub" from the tail and
00506     // everything up to and including the "/" from the head, leaving
00507     // "compressed" (for example) in transport_name.
00508     std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
00509     transport_name = transport_name.substr(lookup_name.find('/') + 1);
00510 
00511     // If the plugin loads without throwing an exception, add its
00512     // transport name to the list of valid plugins, otherwise ignore
00513     // it.
00514     try
00515     {
00516       boost::shared_ptr<image_transport::SubscriberPlugin> sub = sub_loader.createInstance(lookup_name);
00517       transport_plugin_types_.insert(transport_name);
00518     }
00519     catch (const pluginlib::LibraryLoadException& e)
00520     {
00521     }
00522     catch (const pluginlib::CreateClassException& e)
00523     {
00524     }
00525   }
00526 }
00527 
00528 void DepthCloudDisplay::updateTopic()
00529 {
00530   unsubscribe();
00531   reset();
00532   subscribe();
00533   context_->queueRender();
00534 }
00535 
00536 void DepthCloudDisplay::fillTransportOptionList(EnumProperty* property)
00537 {
00538   property->clearOptions();
00539 
00540   std::vector<std::string> choices;
00541 
00542   choices.push_back("raw");
00543 
00544   // Loop over all current ROS topic names
00545   ros::master::V_TopicInfo topics;
00546   ros::master::getTopics(topics);
00547   ros::master::V_TopicInfo::iterator it = topics.begin();
00548   ros::master::V_TopicInfo::iterator end = topics.end();
00549   for (; it != end; ++it)
00550   {
00551     // If the beginning of this topic name is the same as topic_,
00552     // and the whole string is not the same,
00553     // and the next character is /
00554     // and there are no further slashes from there to the end,
00555     // then consider this a possible transport topic.
00556     const ros::master::TopicInfo& ti = *it;
00557     const std::string& topic_name = ti.name;
00558     const std::string& topic = depth_topic_property_->getStdString();
00559 
00560     if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/'
00561         && topic_name.find('/', topic.size() + 1) == std::string::npos)
00562     {
00563       std::string transport_type = topic_name.substr(topic.size() + 1);
00564 
00565       // If the transport type string found above is in the set of
00566       // supported transport type plugins, add it to the list.
00567       if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end())
00568       {
00569         choices.push_back(transport_type);
00570       }
00571     }
00572   }
00573 
00574   for (size_t i = 0; i < choices.size(); i++)
00575   {
00576     property->addOptionStd(choices[i]);
00577   }
00578 }
00579 
00580 void DepthCloudDisplay::fixedFrameChanged()
00581 {
00582   Display::reset();
00583 }
00584 
00585 } // namespace rviz
00586 
00587 #include <pluginlib/class_list_macros.h>
00588 
00589 PLUGINLIB_EXPORT_CLASS( rviz::DepthCloudDisplay, rviz::Display)
00590 


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