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