image_display_base.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, 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 
30 #include <boost/algorithm/string/erase.hpp>
31 #include <boost/foreach.hpp>
32 #include <boost/shared_ptr.hpp>
33 
35 
37 
38 #include <rviz/validate_floats.h>
39 
41 
42 namespace rviz
43 {
44 ImageDisplayBase::ImageDisplayBase() : Display(), sub_(), tf_filter_(), messages_received_(0)
45 {
47  "Image Topic", "", QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
48  "sensor_msgs::Image topic to subscribe to.", this, &ImageDisplayBase::updateTopic);
49 
50  transport_property_ = new EnumProperty("Transport Hint", "raw", "Preferred method of sending images.",
52 
55 
57  new IntProperty("Queue Size", 2,
58  "Advanced: set the size of the incoming message queue. Increasing this "
59  "is useful if your incoming TF data is delayed significantly from your"
60  " image data, but it can greatly increase memory usage if the messages are big.",
63 
65 
66  unreliable_property_ = new BoolProperty("Unreliable", false, "Prefer UDP topic transport", this,
68 }
69 
71 {
73 }
74 
76 {
79 }
80 
81 void ImageDisplayBase::setTopic(const QString& topic, const QString& datatype)
82 {
83  if (datatype == ros::message_traits::datatype<sensor_msgs::Image>())
84  {
86  topic_property_->setString(topic);
87  }
88  else
89  {
90  int index = topic.lastIndexOf("/");
91  if (index == -1)
92  {
93  ROS_WARN("ImageDisplayBase::setTopic() Invalid topic name: %s", topic.toStdString().c_str());
94  return;
95  }
96  QString transport = topic.mid(index + 1);
97  QString base_topic = topic.mid(0, index);
98 
99  transport_property_->setString(transport);
100  topic_property_->setString(base_topic);
101  }
102 }
103 
104 
105 void ImageDisplayBase::incomingMessage(const sensor_msgs::Image::ConstPtr& msg)
106 {
107  if (!msg || context_->getFrameManager()->getPause())
108  {
109  return;
110  }
111 
113  setStatus(StatusProperty::Ok, "Image", QString::number(messages_received_) + " images received");
114 
115  emitTimeSignal(msg->header.stamp);
116 
117  processMessage(msg);
118 }
119 
120 
121 void ImageDisplayBase::failedMessage(const sensor_msgs::Image::ConstPtr& msg,
123 {
125  context_->getFrameManager()->discoverFailureReason(msg->header.frame_id,
126  msg->header.stamp, "", reason));
127 }
128 
129 
131 {
132  Display::reset();
133  if (tf_filter_)
134  {
135  tf_filter_->clear();
136  }
137 
138  messages_received_ = 0;
139  setStatus(StatusProperty::Warn, "Image", "No Image received");
140 }
141 
143 {
144  uint32_t size = queue_size_property_->getInt();
145  if (tf_filter_)
146  {
147  tf_filter_->setQueueSize(size);
148  subscribe();
149  }
150 }
151 
153 {
154  if (!isEnabled())
155  {
156  return;
157  }
158 
159  try
160  {
161  tf_filter_.reset();
162 
164 
165  if (!topic_property_->getTopicStd().empty() && !transport_property_->getStdString().empty())
166  {
167  // Determine UDP vs TCP transport for user selection.
169  {
170  sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
173  }
174  else
175  {
176  sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
178  }
179 
180 
181  if (targetFrame_.empty())
182  {
183  sub_->registerCallback(
184  boost::bind(&ImageDisplayBase::incomingMessage, this, boost::placeholders::_1));
185  }
186  else
187  {
190  update_nh_));
191  tf_filter_->registerCallback(
192  boost::bind(&ImageDisplayBase::incomingMessage, this, boost::placeholders::_1));
193  tf_filter_->registerFailureCallback(boost::bind(
194  &ImageDisplayBase::failedMessage, this, boost::placeholders::_1, boost::placeholders::_2));
195  }
196  }
197  setStatus(StatusProperty::Ok, "Topic", "OK");
198  }
199  catch (ros::Exception& e)
200  {
201  setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
202  }
203  catch (image_transport::Exception& e)
204  {
205  setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
206  }
207 }
208 
210 {
211  tf_filter_.reset();
212  sub_.reset();
213 }
214 
216 {
217  if (tf_filter_)
218  {
219  tf_filter_->setTargetFrame(fixed_frame_.toStdString());
220  reset();
221  }
222 }
223 
225 {
227  "image_transport", "image_transport::SubscriberPlugin");
228 
229  BOOST_FOREACH (const std::string& lookup_name, sub_loader.getDeclaredClasses())
230  {
231  // lookup_name is formatted as "pkg/transport_sub", for instance
232  // "image_transport/compressed_sub" for the "compressed"
233  // transport. This code removes the "_sub" from the tail and
234  // everything up to and including the "/" from the head, leaving
235  // "compressed" (for example) in transport_name.
236  std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
237  transport_name = transport_name.substr(lookup_name.find('/') + 1);
238 
239  // If the plugin loads without throwing an exception, add its
240  // transport name to the list of valid plugins, otherwise ignore
241  // it.
242  try
243  {
245  transport_plugin_types_.insert(transport_name);
246  }
247  catch (const pluginlib::LibraryLoadException& e)
248  {
249  }
250  catch (const pluginlib::CreateClassException& e)
251  {
252  }
253  }
254 }
255 
257 {
258  unsubscribe();
259  reset();
260  subscribe();
262 }
263 
265 {
266  property->clearOptions();
267 
268  std::vector<std::string> choices;
269 
270  choices.push_back("raw");
271 
272  // Loop over all current ROS topic names
274  ros::master::getTopics(topics);
275  ros::master::V_TopicInfo::iterator it = topics.begin();
276  ros::master::V_TopicInfo::iterator end = topics.end();
277  for (; it != end; ++it)
278  {
279  // If the beginning of this topic name is the same as topic_,
280  // and the whole string is not the same,
281  // and the next character is /
282  // and there are no further slashes from there to the end,
283  // then consider this a possible transport topic.
284  const ros::master::TopicInfo& ti = *it;
285  const std::string& topic_name = ti.name;
286  const std::string& topic = topic_property_->getStdString();
287 
288  if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/' &&
289  topic_name.find('/', topic.size() + 1) == std::string::npos)
290  {
291  std::string transport_type = topic_name.substr(topic.size() + 1);
292 
293  // If the transport type string found above is in the set of
294  // supported transport type plugins, add it to the list.
295  if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end())
296  {
297  choices.push_back(transport_type);
298  }
299  }
300  }
301 
302  for (size_t i = 0; i < choices.size(); i++)
303  {
304  property->addOptionStd(choices[i]);
305  }
306 }
307 
308 } // end namespace rviz
ros::TransportHints::unreliable
TransportHints & unreliable()
rviz::ImageDisplayBase::failedMessage
void failedMessage(const sensor_msgs::Image::ConstPtr &msg, tf2_ros::FilterFailureReason reason)
Callback for messages, whose frame_id cannot resolved.
Definition: image_display_base.cpp:121
rviz::BoolProperty::getBool
virtual bool getBool() const
Definition: bool_property.cpp:48
image_transport::SubscriberFilter
rviz::Display::isEnabled
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:271
rviz::IntProperty::setMin
void setMin(int min)
Definition: int_property.cpp:52
rviz::RosTopicProperty
Definition: ros_topic_property.h:39
rviz::ImageDisplayBase::fixedFrameChanged
void fixedFrameChanged() override
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
Definition: image_display_base.cpp:215
validate_floats.h
rviz::ImageDisplayBase::onInitialize
void onInitialize() override
Override this function to do subclass-specific initialization.
Definition: image_display_base.cpp:75
pluginlib::CreateClassException
rviz::DisplayContext::queueRender
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
rviz::Display::emitTimeSignal
void emitTimeSignal(ros::Time time)
Emit a time signal that other Displays can synchronize to.
Definition: display.cpp:285
tf2_ros::MessageFilter
image_transport::ImageTransport
boost::shared_ptr< image_transport::SubscriberPlugin >
rviz::StatusProperty::Error
@ Error
Definition: status_property.h:46
rviz::EnumProperty::setString
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...
Definition: enum_property.cpp:82
rviz::ImageDisplayBase::unsubscribe
virtual void unsubscribe()
Definition: image_display_base.cpp:209
tf2_ros::filter_failure_reasons::FilterFailureReason
FilterFailureReason
rviz::EditableEnumProperty::setString
virtual void setString(const QString &str)
Definition: editable_enum_property.cpp:74
image_transport::Exception
rviz::ImageDisplayBase::unreliable_property_
BoolProperty * unreliable_property_
Definition: image_display_base.h:133
ros::TransportHints
rviz::EnumProperty::requestOptions
void requestOptions(EnumProperty *property_in_need_of_options)
requestOptions() is emitted each time createEditor() is called.
ros::Exception
rviz::ImageDisplayBase::processMessage
virtual void processMessage(const sensor_msgs::Image::ConstPtr &msg)=0
Implement this to process the contents of a message.
rviz::ImageDisplayBase::tf_filter_
boost::shared_ptr< tf2_ros::MessageFilter< sensor_msgs::Image > > tf_filter_
Definition: image_display_base.h:119
rviz::Display::fixed_frame_
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
rviz::ImageDisplayBase::fillTransportOptionList
void fillTransportOptionList(EnumProperty *property)
Fill list of available and working transport options.
Definition: image_display_base.cpp:264
ros::master::getTopics
ROSCPP_DECL bool getTopics(V_TopicInfo &topics)
rviz::BoolProperty::BoolProperty
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=nullptr)
Definition: bool_property.cpp:36
rviz::Property::connect
QMetaObject::Connection connect(const QObject *receiver, const char *slot, Qt::ConnectionType type=Qt::AutoConnection)
Connect changed() signal to given slot of receiver.
Definition: property.cpp:78
rviz::Display
Definition: display.h:63
rviz::EnumProperty
Enum property.
Definition: enum_property.h:46
rviz::ImageDisplayBase::setTopic
void setTopic(const QString &topic, const QString &datatype) override
Set the ROS topic to listen to for this display.
Definition: image_display_base.cpp:81
rviz::ImageDisplayBase::incomingMessage
void incomingMessage(const sensor_msgs::Image::ConstPtr &msg)
Incoming message callback. Checks if the message pointer is valid, increments messages_received_,...
Definition: image_display_base.cpp:105
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:176
rviz::ImageDisplayBase::it_
boost::scoped_ptr< image_transport::ImageTransport > it_
Definition: image_display_base.h:117
rviz
Definition: add_display_dialog.cpp:54
ros::master::V_TopicInfo
std::vector< TopicInfo > V_TopicInfo
rviz::ImageDisplayBase::~ImageDisplayBase
~ImageDisplayBase() override
Definition: image_display_base.cpp:70
rviz::StringProperty::getStdString
std::string getStdString()
Definition: string_property.h:71
rviz::StatusProperty::Ok
@ Ok
Definition: status_property.h:44
rviz::StatusProperty::Warn
@ Warn
Definition: status_property.h:45
rviz::FrameManager::getPause
bool getPause()
Definition: frame_manager.h:97
rviz::ImageDisplayBase::scanForTransportSubscriberPlugins
void scanForTransportSubscriberPlugins()
Definition: image_display_base.cpp:224
rviz::ImageDisplayBase::ImageDisplayBase
ImageDisplayBase()
Constructor.
Definition: image_display_base.cpp:44
ROS_WARN
#define ROS_WARN(...)
rviz::ImageDisplayBase::updateTopic
virtual void updateTopic()
Update topic and resubscribe.
Definition: image_display_base.cpp:256
pluginlib::ClassLoader
class_loader.hpp
rviz::DisplayContext::getFrameManager
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
subscriber_plugin.h
rviz::RosTopicProperty::getTopicStd
std::string getTopicStd() const
Definition: ros_topic_property.h:85
rviz::ImageDisplayBase::targetFrame_
std::string targetFrame_
Definition: image_display_base.h:121
rviz::Display::setStatusStd
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
rviz::Display::context_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz....
Definition: display.h:287
pluginlib::ClassLoader::createInstance
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
rviz::ImageDisplayBase::sub_
boost::shared_ptr< image_transport::SubscriberFilter > sub_
Definition: image_display_base.h:118
rviz::ImageDisplayBase::reset
void reset() override
Reset display.
Definition: image_display_base.cpp:130
rviz::ImageDisplayBase::updateQueueSize
virtual void updateQueueSize()
Update queue size of tf filter
Definition: image_display_base.cpp:142
rviz::StringProperty::setStdString
bool setStdString(const std::string &std_str)
Definition: string_property.h:85
datatype
const char * datatype()
rviz::Display::reset
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:290
rviz::IntProperty::getInt
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:96
pluginlib::LibraryLoadException
image_transport::TransportHints
rviz::ImageDisplayBase::topic_property_
RosTopicProperty * topic_property_
Definition: image_display_base.h:125
ros::master::TopicInfo
image_display_base.h
rviz::ImageDisplayBase::queue_size_property_
IntProperty * queue_size_property_
Definition: image_display_base.h:127
rviz::ImageDisplayBase::messages_received_
uint32_t messages_received_
Definition: image_display_base.h:123
rviz::ImageDisplayBase::transport_property_
EnumProperty * transport_property_
Definition: image_display_base.h:126
rviz::ImageDisplayBase::transport_plugin_types_
std::set< std::string > transport_plugin_types_
Definition: image_display_base.h:131
rviz::DisplayContext::getTF2BufferPtr
std::shared_ptr< tf2_ros::Buffer > getTF2BufferPtr() const
Convenience function: returns getFrameManager()->getTF2BufferPtr().
Definition: display_context.h:98
rviz::Display::update_nh_
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
rviz::ImageDisplayBase::subscribe
virtual void subscribe()
ROS topic management.
Definition: image_display_base.cpp:152
rviz::FrameManager::discoverFailureReason
std::string discoverFailureReason(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, tf2_ros::FilterFailureReason reason)
Definition: frame_manager.cpp:311
ros::master::TopicInfo::name
std::string name
rviz::IntProperty
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
pluginlib::ClassLoader::getDeclaredClasses
std::vector< std::string > getDeclaredClasses()


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Dec 13 2024 03:31:02