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 
46  Display()
47  , sub_()
48  , tf_filter_()
49  , messages_received_(0)
50 {
51  topic_property_ = new RosTopicProperty("Image Topic", "",
52  QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
53  "sensor_msgs::Image topic to subscribe to.", this, SLOT( updateTopic() ));
54 
55  transport_property_ = new EnumProperty("Transport Hint", "raw", "Preferred method of sending images.", this,
56  SLOT( updateTopic() ));
57 
58  connect(transport_property_, SIGNAL( requestOptions( EnumProperty* )), this,
60 
61  queue_size_property_ = new IntProperty( "Queue Size", 2,
62  "Advanced: set the size of the incoming message queue. Increasing this "
63  "is useful if your incoming TF data is delayed significantly from your"
64  " image data, but it can greatly increase memory usage if the messages are big.",
65  this, SLOT( updateQueueSize() ));
67 
69 
70  unreliable_property_ = new BoolProperty( "Unreliable", false,
71  "Prefer UDP topic transport",
72  this,
73  SLOT( updateTopic() ));
74 
75 }
76 
78 {
79  unsubscribe();
80 }
81 
83 {
86 }
87 
88 void ImageDisplayBase::setTopic( const QString &topic, const QString &datatype )
89 {
90  if ( datatype == ros::message_traits::datatype<sensor_msgs::Image>() )
91  {
93  topic_property_->setString( topic );
94  }
95  else
96  {
97  int index = topic.lastIndexOf("/");
98  if ( index == -1 )
99  {
100  ROS_WARN("ImageDisplayBase::setTopic() Invalid topic name: %s",
101  topic.toStdString().c_str());
102  return;
103  }
104  QString transport = topic.mid(index + 1);
105  QString base_topic = topic.mid(0, index);
106 
107  transport_property_->setString( transport );
108  topic_property_->setString( base_topic );
109  }
110 }
111 
112 
113 void ImageDisplayBase::incomingMessage(const sensor_msgs::Image::ConstPtr& msg)
114 {
115  if (!msg || context_->getFrameManager()->getPause() )
116  {
117  return;
118  }
119 
121  setStatus(StatusProperty::Ok, "Image", QString::number(messages_received_) + " images received");
122 
123  emitTimeSignal( msg->header.stamp );
124 
125  processMessage(msg);
126 }
127 
128 
130 {
131  Display::reset();
132  if (tf_filter_)
133  tf_filter_->clear();
134  messages_received_ = 0;
135 }
136 
138 {
139  uint32_t size = queue_size_property_->getInt();
140  if (tf_filter_)
141  tf_filter_->setQueueSize(size);
142 }
143 
145 {
146  if (!isEnabled())
147  {
148  return;
149  }
150 
151  try
152  {
153 
154  tf_filter_.reset();
155 
157 
158  if (!topic_property_->getTopicStd().empty() && !transport_property_->getStdString().empty() )
159  {
160 
161  // Determine UDP vs TCP transport for user selection.
163  {
164  sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
166  }
167  else{
168  sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
170  }
171 
172 
173  if (targetFrame_.empty())
174  {
175  sub_->registerCallback(boost::bind(&ImageDisplayBase::incomingMessage, this, _1));
176  }
177  else
178  {
180  tf_filter_->registerCallback(boost::bind(&ImageDisplayBase::incomingMessage, this, _1));
181  }
182  }
183  setStatus(StatusProperty::Ok, "Topic", "OK");
184  }
185  catch (ros::Exception& e)
186  {
187  setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
188  }
189  catch (image_transport::Exception& e)
190  {
191  setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
192  }
193 
194  messages_received_ = 0;
195  setStatus(StatusProperty::Warn, "Image", "No Image received");
196 }
197 
199 {
200  tf_filter_.reset();
202 }
203 
205 {
206  if (tf_filter_)
207  {
208  tf_filter_->setTargetFrame(fixed_frame_.toStdString());
209  reset();
210  }
211 }
212 
214 {
216  "image_transport::SubscriberPlugin");
217 
218  BOOST_FOREACH( const std::string& lookup_name, sub_loader.getDeclaredClasses() )
219  {
220  // lookup_name is formatted as "pkg/transport_sub", for instance
221  // "image_transport/compressed_sub" for the "compressed"
222  // transport. This code removes the "_sub" from the tail and
223  // everything up to and including the "/" from the head, leaving
224  // "compressed" (for example) in transport_name.
225  std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
226  transport_name = transport_name.substr(lookup_name.find('/') + 1);
227 
228  // If the plugin loads without throwing an exception, add its
229  // transport name to the list of valid plugins, otherwise ignore
230  // it.
231  try
232  {
233  boost::shared_ptr<image_transport::SubscriberPlugin> sub = sub_loader.createInstance(lookup_name);
234  transport_plugin_types_.insert(transport_name);
235  }
236  catch (const pluginlib::LibraryLoadException& e)
237  {
238  }
239  catch (const pluginlib::CreateClassException& e)
240  {
241  }
242  }
243 }
244 
246 {
247  unsubscribe();
248  reset();
249  subscribe();
251 }
252 
254 {
255  property->clearOptions();
256 
257  std::vector<std::string> choices;
258 
259  choices.push_back("raw");
260 
261  // Loop over all current ROS topic names
263  ros::master::getTopics(topics);
264  ros::master::V_TopicInfo::iterator it = topics.begin();
265  ros::master::V_TopicInfo::iterator end = topics.end();
266  for (; it != end; ++it)
267  {
268  // If the beginning of this topic name is the same as topic_,
269  // and the whole string is not the same,
270  // and the next character is /
271  // and there are no further slashes from there to the end,
272  // then consider this a possible transport topic.
273  const ros::master::TopicInfo& ti = *it;
274  const std::string& topic_name = ti.name;
275  const std::string& topic = topic_property_->getStdString();
276 
277  if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/'
278  && topic_name.find('/', topic.size() + 1) == std::string::npos)
279  {
280  std::string transport_type = topic_name.substr(topic.size() + 1);
281 
282  // If the transport type string found above is in the set of
283  // supported transport type plugins, add it to the list.
284  if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end())
285  {
286  choices.push_back(transport_type);
287  }
288  }
289  }
290 
291  for (size_t i = 0; i < choices.size(); i++)
292  {
293  property->addOptionStd(choices[i]);
294  }
295 }
296 
297 } // end namespace rviz
virtual void setString(const QString &str)
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
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:256
void emitTimeSignal(ros::Time time)
Emit a time signal that other Displays can synchronize to.
Definition: display.cpp:295
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:73
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:269
ROSCPP_DECL bool getTopics(V_TopicInfo &topics)
void setMin(int min)
std::vector< TopicInfo > V_TopicInfo
#define ROS_WARN(...)
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:38
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
std::string getStdString()
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:281
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
BoolProperty * unreliable_property_
virtual void processMessage(const sensor_msgs::Image::ConstPtr &msg)=0
Implement this to process the contents of a message.
IntProperty * queue_size_property_
TransportHints & unreliable()
virtual void subscribe()
ROS topic management.
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:300
std::vector< std::string > getDeclaredClasses()
virtual void updateQueueSize()
Update queue size of tf filter.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
virtual void onInitialize()
Override this function to do subclass-specific initialization.
virtual void reset()
Reset display.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
EnumProperty * transport_property_
RosTopicProperty * topic_property_
virtual void updateTopic()
Update topic and resubscribe.
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > tf_filter_
std::set< std::string > transport_plugin_types_
void incomingMessage(const sensor_msgs::Image::ConstPtr &msg)
Incoming message callback. Checks if the message pointer is valid, increments messages_received_, then calls processMessage().
std::string getTopicStd() const
boost::shared_ptr< image_transport::SubscriberFilter > sub_
bool setStdString(const std::string &std_str)
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:47
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
boost::scoped_ptr< image_transport::ImageTransport > it_
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:186
virtual void setTopic(const QString &topic, const QString &datatype)
Set the ROS topic to listen to for this display.


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41