depth_cloud_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 #include <QObject>
30 
31 #include "depth_cloud_display.h"
34 #include "rviz/validate_floats.h"
35 
40 #include "rviz/frame_manager.h"
41 
42 #include <tf2_ros/buffer.h>
43 
44 #include <boost/bind.hpp>
45 #include <boost/algorithm/string/erase.hpp>
46 #include <boost/foreach.hpp>
47 #include <boost/shared_ptr.hpp>
48 
49 #include <OgreSceneNode.h>
50 #include <OgreSceneManager.h>
51 
55 
57 
58 #include "depth_cloud_mld.h"
59 
60 #include <sstream>
61 #include <string>
62 #include <math.h>
63 
65 
66 namespace rviz
67 {
69  : rviz::Display()
70  , messages_received_(0)
71  , depthmap_sub_()
72  , rgb_sub_()
73  , cam_info_sub_()
74  , queue_size_(5)
75  , ml_depth_data_(new MultiLayerDepth())
76  , angular_thres_(0.5f)
77  , trans_thres_(0.01f)
78 
79 {
80  // Depth map properties
81  QRegExp depth_filter("depth");
82  depth_filter.setCaseSensitivity(Qt::CaseInsensitive);
83 
85  new Property("Topic Filter", true,
86  "List only topics with names that relate to depth and color images", this,
87  SLOT(updateTopicFilter()));
88 
90  "Depth Map Topic", "", QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
91  "sensor_msgs::Image topic to subscribe to.", depth_filter, this, SLOT(updateTopic()));
92 
94  new EnumProperty("Depth Map Transport Hint", "raw", "Preferred method of sending images.", this,
95  SLOT(updateTopic()));
96 
97  connect(depth_transport_property_, SIGNAL(requestOptions(EnumProperty*)), this,
99 
101 
102  // color image properties
103  QRegExp color_filter("color|rgb|bgr|gray|mono");
104  color_filter.setCaseSensitivity(Qt::CaseInsensitive);
105 
107  "Color Image Topic", "",
108  QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
109  "sensor_msgs::Image topic to subscribe to.", color_filter, this, SLOT(updateTopic()));
110 
112  "Color Transport Hint", "raw", "Preferred method of sending images.", this, SLOT(updateTopic()));
113 
114 
115  connect(color_transport_property_, SIGNAL(requestOptions(EnumProperty*)), this,
117 
119 
120  // Queue size property
122  new IntProperty("Queue Size", queue_size_,
123  "Advanced: set the size of the incoming message queue. Increasing this "
124  "is useful if your incoming TF data is delayed significantly from your"
125  " image data, but it can greatly increase memory usage if the messages are big.",
126  this, SLOT(updateQueueSize()));
128 
130  "Auto Size", true,
131  "Automatically scale each point based on its depth value and the camera parameters.", this,
132  SLOT(updateUseAutoSize()), this);
133 
135  new FloatProperty("Auto Size Factor", 1, "Scaling factor to be applied to the auto size.",
138 
140  new BoolProperty("Occlusion Compensation", false,
141  "Keep points alive after they have been occluded by a closer point. Points are "
142  "removed after a timeout or when the camera frame moves.",
143  this, SLOT(updateUseOcclusionCompensation()), this);
144 
146  new FloatProperty("Occlusion Time-Out", 30.0f,
147  "Amount of seconds before removing occluded points from the depth cloud",
149 }
150 
152 {
155 
156  // Instantiate PointCloudCommon class for displaying point clouds
158 
161 
162  // Scan for available transport plugins
164 
167 }
168 
170 {
171  if (initialized())
172  {
173  unsubscribe();
174  delete pointcloud_common_;
175  }
176 
177  if (ml_depth_data_)
178  {
179  delete ml_depth_data_;
180  }
181 }
182 
183 void DepthCloudDisplay::setTopic(const QString& topic, const QString& datatype)
184 {
185  // Copied from ImageDisplayBase::setTopic()
186  if (datatype == ros::message_traits::datatype<sensor_msgs::Image>())
187  {
190  }
191  else
192  {
193  int index = topic.lastIndexOf("/");
194  if (index == -1)
195  {
196  ROS_WARN("DepthCloudDisplay::setTopic() Invalid topic name: %s", topic.toStdString().c_str());
197  return;
198  }
199  QString transport = topic.mid(index + 1);
200  QString base_topic = topic.mid(0, index);
201 
203  depth_topic_property_->setString(base_topic);
204  }
205 }
206 
208 {
210 }
211 
213 {
214  bool use_auto_size = use_auto_size_property_->getBool();
216  pointcloud_common_->setAutoSize(use_auto_size);
217  auto_size_factor_property_->setHidden(!use_auto_size);
218  if (use_auto_size)
220 }
221 
223 {
224 }
225 
227 {
228  bool enabled = topic_filter_property_->getValue().toBool();
231 }
232 
234 {
235  bool use_occlusion_compensation = use_occlusion_compensation_property_->getBool();
236  occlusion_shadow_timeout_property_->setHidden(!use_occlusion_compensation);
237 
238  if (use_occlusion_compensation)
239  {
243  }
244  else
245  {
247  }
248 }
249 
251 {
252  float occlusion_timeout = occlusion_shadow_timeout_property_->getFloat();
253  ml_depth_data_->setShadowTimeOut(occlusion_timeout);
254 }
255 
257 {
258  subscribe();
259 }
260 
262 {
263  unsubscribe();
264 
266 
267  clear();
268 }
269 
271 {
272  if (!isEnabled())
273  {
274  return;
275  }
276 
277  try
278  {
279  // reset all message filters
281  depthmap_tf_filter_.reset();
285 
286  std::string depthmap_topic = depth_topic_property_->getTopicStd();
287  std::string color_topic = color_topic_property_->getTopicStd();
288 
289  std::string depthmap_transport = depth_transport_property_->getStdString();
290  std::string color_transport = color_transport_property_->getStdString();
291 
292  if (!depthmap_topic.empty() && !depthmap_transport.empty())
293  {
294  // subscribe to depth map topic
295  depthmap_sub_->subscribe(*depthmap_it_, depthmap_topic, queue_size_,
296  image_transport::TransportHints(depthmap_transport));
297 
300  threaded_nh_));
301 
302  // subscribe to CameraInfo topic
303  std::string info_topic = image_transport::getCameraInfoTopic(depthmap_topic);
304  cam_info_sub_->subscribe(threaded_nh_, info_topic, queue_size_);
305  cam_info_sub_->registerCallback(boost::bind(&DepthCloudDisplay::caminfoCallback, this, _1));
306 
307  if (!color_topic.empty() && !color_transport.empty())
308  {
309  // subscribe to color image topic
310  rgb_sub_->subscribe(*rgb_it_, color_topic, queue_size_,
311  image_transport::TransportHints(color_transport));
312 
313  // connect message filters to synchronizer
315  sync_depth_color_->setInterMessageLowerBound(0, ros::Duration(0.5));
316  sync_depth_color_->setInterMessageLowerBound(1, ros::Duration(0.5));
317  sync_depth_color_->registerCallback(
318  boost::bind(&DepthCloudDisplay::processMessage, this, _1, _2));
319 
321  }
322  else
323  {
324  depthmap_tf_filter_->registerCallback(boost::bind(&DepthCloudDisplay::processMessage, this, _1));
325  }
326  }
327  }
328  catch (ros::Exception& e)
329  {
330  setStatus(StatusProperty::Error, "Message", QString("Error subscribing: ") + e.what());
331  }
333  {
334  setStatus(StatusProperty::Error, "Message", QString("Error subscribing: ") + e.what());
335  }
336 }
337 
338 void DepthCloudDisplay::caminfoCallback(sensor_msgs::CameraInfo::ConstPtr msg)
339 {
340  boost::mutex::scoped_lock lock(cam_info_mutex_);
341  cam_info_ = msg;
342 }
343 
345 {
346  clear();
347 
348  try
349  {
350  // reset all filters
352  depthmap_tf_filter_.reset();
353  depthmap_sub_.reset();
354  rgb_sub_.reset();
355  cam_info_sub_.reset();
356  }
357  catch (ros::Exception& e)
358  {
359  setStatus(StatusProperty::Error, "Message", QString("Error unsubscribing: ") + e.what());
360  }
361 }
362 
363 
365 {
366  boost::mutex::scoped_lock lock(mutex_);
367 
369 }
370 
371 
372 void DepthCloudDisplay::update(float wall_dt, float ros_dt)
373 {
374  boost::mutex::scoped_lock lock(mutex_);
375 
376  pointcloud_common_->update(wall_dt, ros_dt);
377 }
378 
379 
381 {
382  clear();
383  messages_received_ = 0;
384  setStatus(StatusProperty::Ok, "Depth Map", "0 depth maps received");
385  setStatus(StatusProperty::Ok, "Message", "Ok");
386 }
387 
388 void DepthCloudDisplay::processMessage(sensor_msgs::ImageConstPtr depth_msg)
389 {
390  processMessage(depth_msg, sensor_msgs::ImageConstPtr());
391 }
392 
393 void DepthCloudDisplay::processMessage(sensor_msgs::ImageConstPtr depth_msg,
394  sensor_msgs::ImageConstPtr rgb_msg)
395 {
397  {
398  return;
399  }
400 
401  std::ostringstream s;
402 
404  setStatus(StatusProperty::Ok, "Depth Map",
405  QString::number(messages_received_) + " depth maps received");
406  setStatus(StatusProperty::Ok, "Message", "Ok");
407 
408  sensor_msgs::CameraInfo::ConstPtr cam_info;
409  {
410  boost::mutex::scoped_lock lock(cam_info_mutex_);
411  cam_info = cam_info_;
412  }
413 
414  if (!cam_info || !depth_msg)
415  {
416  return;
417  }
418 
419  s.str("");
420  s << depth_msg->width << " x " << depth_msg->height;
421  setStatusStd(StatusProperty::Ok, "Depth Image Size", s.str());
422 
423  if (rgb_msg)
424  {
425  s.str("");
426  s << rgb_msg->width << " x " << rgb_msg->height;
427  setStatusStd(StatusProperty::Ok, "Image Size", s.str());
428 
429  if (depth_msg->header.frame_id != rgb_msg->header.frame_id)
430  {
431  std::stringstream errorMsg;
432  errorMsg << "Depth image frame id [" << depth_msg->header.frame_id.c_str()
433  << "] doesn't match color image frame id [" << rgb_msg->header.frame_id.c_str() << "]";
434  setStatusStd(StatusProperty::Warn, "Message", errorMsg.str());
435  }
436  }
437 
439  {
440  float f = cam_info->K[0];
441  float bx = cam_info->binning_x > 0 ? cam_info->binning_x : 1.0;
444  }
445 
446  bool use_occlusion_compensation = use_occlusion_compensation_property_->getBool();
447 
448  if (use_occlusion_compensation)
449  {
450  // reset depth cloud display if camera moves
451  Ogre::Quaternion orientation;
452  Ogre::Vector3 position;
453 
454  if (!context_->getFrameManager()->getTransform(depth_msg->header, position, orientation))
455  {
456  setStatus(StatusProperty::Error, "Message",
457  QString("Failed to transform from frame [") + depth_msg->header.frame_id.c_str() +
458  QString("] to frame [") + context_->getFrameManager()->getFixedFrame().c_str() +
459  QString("]"));
460  return;
461  }
462  else
463  {
464  Ogre::Radian angle;
465  Ogre::Vector3 axis;
466 
467  (current_orientation_.Inverse() * orientation).ToAngleAxis(angle, axis);
468 
469  float angle_deg = angle.valueDegrees();
470  if (angle_deg >= 180.0f)
471  angle_deg -= 180.0f;
472  if (angle_deg < -180.0f)
473  angle_deg += 180.0f;
474 
475  if (trans_thres_ == 0.0 || angular_thres_ == 0.0 ||
476  (position - current_position_).length() > trans_thres_ || angle_deg > angular_thres_)
477  {
478  // camera orientation/position changed
479  current_position_ = position;
480  current_orientation_ = orientation;
481 
482  // reset multi-layered depth image
484  }
485  }
486  }
487 
488  try
489  {
490  sensor_msgs::PointCloud2Ptr cloud_msg =
491  ml_depth_data_->generatePointCloudFromDepth(depth_msg, rgb_msg, cam_info);
492 
493  if (!cloud_msg.get())
494  {
495  throw MultiLayerDepthException("generatePointCloudFromDepth() returned zero.");
496  }
497  cloud_msg->header = depth_msg->header;
498 
499  // add point cloud message to pointcloud_common to be visualized
500  pointcloud_common_->addMessage(cloud_msg);
501  }
502  catch (MultiLayerDepthException& e)
503  {
504  setStatus(StatusProperty::Error, "Message", QString("Error updating depth cloud: ") + e.what());
505  }
506 }
507 
508 
510 {
512  "image_transport", "image_transport::SubscriberPlugin");
513 
514  BOOST_FOREACH (const std::string& lookup_name, sub_loader.getDeclaredClasses())
515  {
516  // lookup_name is formatted as "pkg/transport_sub", for instance
517  // "image_transport/compressed_sub" for the "compressed"
518  // transport. This code removes the "_sub" from the tail and
519  // everything up to and including the "/" from the head, leaving
520  // "compressed" (for example) in transport_name.
521  std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
522  transport_name = transport_name.substr(lookup_name.find('/') + 1);
523 
524  // If the plugin loads without throwing an exception, add its
525  // transport name to the list of valid plugins, otherwise ignore
526  // it.
527  try
528  {
530  transport_plugin_types_.insert(transport_name);
531  }
532  catch (const pluginlib::LibraryLoadException& e)
533  {
534  }
535  catch (const pluginlib::CreateClassException& e)
536  {
537  }
538  }
539 }
540 
542 {
543  unsubscribe();
544  reset();
545  subscribe();
547 }
548 
550 {
551  property->clearOptions();
552 
553  std::vector<std::string> choices;
554 
555  choices.push_back("raw");
556 
557  // Loop over all current ROS topic names
559  ros::master::getTopics(topics);
560  ros::master::V_TopicInfo::iterator it = topics.begin();
561  ros::master::V_TopicInfo::iterator end = topics.end();
562  for (; it != end; ++it)
563  {
564  // If the beginning of this topic name is the same as topic_,
565  // and the whole string is not the same,
566  // and the next character is /
567  // and there are no further slashes from there to the end,
568  // then consider this a possible transport topic.
569  const ros::master::TopicInfo& ti = *it;
570  const std::string& topic_name = ti.name;
571  const std::string& topic = depth_topic_property_->getStdString();
572 
573  if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/' &&
574  topic_name.find('/', topic.size() + 1) == std::string::npos)
575  {
576  std::string transport_type = topic_name.substr(topic.size() + 1);
577 
578  // If the transport type string found above is in the set of
579  // supported transport type plugins, add it to the list.
580  if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end())
581  {
582  choices.push_back(transport_type);
583  }
584  }
585  }
586 
587  for (size_t i = 0; i < choices.size(); i++)
588  {
589  property->addOptionStd(choices[i]);
590  }
591 }
592 
594 {
595  Display::reset();
596 }
597 
598 } // namespace rviz
599 
601 
Ogre::Quaternion current_orientation_
const char * what() const override
boost::shared_ptr< SynchronizerDepthColor > sync_depth_color_
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:163
void setMin(float min)
virtual void processMessage(sensor_msgs::Image::ConstPtr msg)
boost::scoped_ptr< image_transport::ImageTransport > rgb_it_
virtual void setString(const QString &str)
void onDisable() override
Derived classes override this to do the actual work of disabling themselves.
virtual void expand()
Expand (show the children of) this Property.
Definition: property.cpp:555
void fixedFrameChanged() override
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
f
void fillTransportOptionList(EnumProperty *property)
Fill list of available and working transport options.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
Definition: property.cpp:134
XmlRpcServer s
FloatProperty * point_world_size_property_
FloatProperty * auto_size_factor_property_
MultiLayerDepth * ml_depth_data_
message_filters::Synchronizer< SyncPolicyDepthColor > SynchronizerDepthColor
void enableOcclusionCompensation(bool occlusion_compensation)
EnumProperty * xyz_transformer_property_
ROSCPP_DECL bool getTopics(V_TopicInfo &topics)
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
void setMin(int min)
void onInitialize() override
Override this function to do subclass-specific initialization.
std::vector< TopicInfo > V_TopicInfo
Property specialized to enforce floating point max/min.
sensor_msgs::CameraInfo::ConstPtr cam_info_
#define ROS_WARN(...)
std::string getTopicStd() const
boost::shared_ptr< image_transport::SubscriberFilter > depthmap_sub_
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
std::string getStdString()
Displays a point cloud of type sensor_msgs::PointCloud.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
std::set< std::string > transport_plugin_types_
bool setFloat(float new_value)
Float-typed "SLOT" version of setValue().
boost::shared_ptr< tf2_ros::MessageFilter< sensor_msgs::Image > > depthmap_tf_filter_
boost::scoped_ptr< image_transport::ImageTransport > depthmap_it_
void caminfoCallback(sensor_msgs::CameraInfo::ConstPtr msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
ApproximateTime< sensor_msgs::Image, sensor_msgs::Image > SyncPolicyDepthColor
boost::shared_ptr< message_filters::Subscriber< sensor_msgs::CameraInfo > > cam_info_sub_
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:287
EnumProperty * color_transformer_property_
std::vector< std::string > getDeclaredClasses()
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=nullptr, const char *changed_slot=nullptr, QObject *receiver=nullptr)
Constructor.
Definition: property.cpp:58
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
RosFilteredTopicProperty * color_topic_property_
IMAGE_TRANSPORT_DECL std::string getCameraInfoTopic(const std::string &base_topic)
const std::string & getFixedFrame()
Return the current fixed frame name.
BoolProperty * use_occlusion_compensation_property_
PointCloudCommon * pointcloud_common_
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:74
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=nullptr, const char *changed_slot=nullptr, QObject *receiver=nullptr)
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:543
virtual std::shared_ptr< tf2_ros::Buffer > getTF2BufferPtr() const =0
Convenience function: returns getFrameManager()->getTF2BufferPtr().
sensor_msgs::PointCloud2Ptr generatePointCloudFromDepth(sensor_msgs::ImageConstPtr depth_msg, sensor_msgs::ImageConstPtr color_msg, sensor_msgs::CameraInfoConstPtr camera_info_msg)
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:268
ros::NodeHandle threaded_nh_
A NodeHandle whose CallbackQueue is run from a different thread than the GUI.
Definition: display.h:305
void setShadowTimeOut(double time_out)
virtual float getFloat() const
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
void onEnable() override
Derived classes override this to do the actual work of enabling themselves.
void setAutoSize(bool auto_size)
virtual void updateUseOcclusionCompensation()
EnumProperty * depth_transport_property_
FloatProperty * occlusion_shadow_timeout_property_
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
EnumProperty * color_transport_property_
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:398
virtual QVariant getValue() const
Return the value of this Property as a QVariant. If the value has never been set, an invalid QVariant...
Definition: property.cpp:150
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:275
void setTopic(const QString &topic, const QString &datatype) override
Set the ROS topic to listen to for this display.
boost::shared_ptr< image_transport::SubscriberFilter > rgb_sub_
virtual bool getBool() const
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:433
bool setStdString(const std::string &std_str)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void reset() override
Called to tell the display to clear its state.
virtual void setString(const QString &str)
Set the value of this property to the given string. Does not force the value to be one of the list of...
Enum property.
Definition: enum_property.h:46
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:175
BoolProperty * use_auto_size_property_
void update(float wall_dt, float ros_dt)
RosFilteredTopicProperty * depth_topic_property_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Aug 9 2022 02:06:08