image_display_base.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, 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 
00030 #include <boost/algorithm/string/erase.hpp>
00031 #include <boost/foreach.hpp>
00032 #include <boost/shared_ptr.hpp>
00033 
00034 #include <pluginlib/class_loader.h>
00035 
00036 #include <image_transport/subscriber_plugin.h>
00037 
00038 #include "rviz/validate_floats.h"
00039 
00040 #include "rviz/image/image_display_base.h"
00041 
00042 namespace rviz
00043 {
00044 
00045 ImageDisplayBase::ImageDisplayBase() :
00046     Display()
00047     , sub_()
00048     , tf_filter_()
00049     , messages_received_(0)
00050 {
00051   topic_property_ = new RosTopicProperty("Image Topic", "",
00052                                          QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
00053                                          "sensor_msgs::Image topic to subscribe to.", this, SLOT( updateTopic() ));
00054 
00055   transport_property_ = new EnumProperty("Transport Hint", "raw", "Preferred method of sending images.", this,
00056                                          SLOT( updateTopic() ));
00057 
00058   connect(transport_property_, SIGNAL( requestOptions( EnumProperty* )), this,
00059           SLOT( fillTransportOptionList( EnumProperty* )));
00060 
00061   queue_size_property_ = new IntProperty( "Queue Size", 2,
00062                                           "Advanced: set the size of the incoming message queue.  Increasing this "
00063                                           "is useful if your incoming TF data is delayed significantly from your"
00064                                           " image data, but it can greatly increase memory usage if the messages are big.",
00065                                           this, SLOT( updateQueueSize() ));
00066   queue_size_property_->setMin( 1 );
00067 
00068   transport_property_->setStdString("raw");
00069 
00070   unreliable_property_ = new BoolProperty( "Unreliable", false,
00071                                            "Prefer UDP topic transport",
00072                                            this,
00073                                            SLOT( updateTopic() ));
00074 
00075 }
00076 
00077 ImageDisplayBase::~ImageDisplayBase()
00078 {
00079   unsubscribe();
00080 }
00081 
00082 void ImageDisplayBase::onInitialize()
00083 {
00084   it_.reset( new image_transport::ImageTransport( update_nh_ ));
00085   scanForTransportSubscriberPlugins();
00086 }
00087 
00088 void ImageDisplayBase::setTopic( const QString &topic, const QString &datatype )
00089 {
00090   if ( datatype == ros::message_traits::datatype<sensor_msgs::Image>() )
00091   {
00092     transport_property_->setStdString( "raw" );
00093     topic_property_->setString( topic );
00094   }
00095   else
00096   {
00097     int index = topic.lastIndexOf("/");
00098     if ( index == -1 )
00099     {
00100       ROS_WARN("ImageDisplayBase::setTopic() Invalid topic name: %s",
00101                topic.toStdString().c_str());
00102       return;
00103     }
00104     QString transport = topic.mid(index + 1);
00105     QString base_topic = topic.mid(0, index);
00106 
00107     transport_property_->setString( transport );
00108     topic_property_->setString( base_topic );
00109   }
00110 }
00111 
00112 
00113 void ImageDisplayBase::incomingMessage(const sensor_msgs::Image::ConstPtr& msg)
00114 {
00115   if (!msg || context_->getFrameManager()->getPause() )
00116   {
00117     return;
00118   }
00119 
00120   ++messages_received_;
00121   setStatus(StatusProperty::Ok, "Image", QString::number(messages_received_) + " images received");
00122 
00123   emitTimeSignal( msg->header.stamp );
00124 
00125   processMessage(msg);
00126 }
00127 
00128 
00129 void ImageDisplayBase::reset()
00130 {
00131   Display::reset();
00132   if (tf_filter_)
00133     tf_filter_->clear();
00134   messages_received_ = 0;
00135 }
00136 
00137 void ImageDisplayBase::updateQueueSize()
00138 {
00139   uint32_t size = queue_size_property_->getInt();
00140   if (tf_filter_)
00141     tf_filter_->setQueueSize(size);
00142 }
00143 
00144 void ImageDisplayBase::subscribe()
00145 {
00146   if (!isEnabled())
00147   {
00148     return;
00149   }
00150 
00151   try
00152   {
00153 
00154     tf_filter_.reset();
00155 
00156     sub_.reset(new image_transport::SubscriberFilter());
00157 
00158     if (!topic_property_->getTopicStd().empty() && !transport_property_->getStdString().empty() )
00159     {
00160 
00161         // Determine UDP vs TCP transport for user selection.
00162         if (unreliable_property_->getBool())
00163         {
00164             sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
00165                             image_transport::TransportHints(transport_property_->getStdString(), ros::TransportHints().unreliable()));
00166         }
00167         else{
00168             sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
00169                             image_transport::TransportHints(transport_property_->getStdString()));
00170         }
00171 
00172 
00173       if (targetFrame_.empty())
00174       {
00175         sub_->registerCallback(boost::bind(&ImageDisplayBase::incomingMessage, this, _1));
00176       }
00177       else
00178       {
00179         tf_filter_.reset( new tf::MessageFilter<sensor_msgs::Image>(*sub_, (tf::Transformer&)*(context_->getTFClient()), targetFrame_, (uint32_t)queue_size_property_->getInt(), update_nh_));
00180         tf_filter_->registerCallback(boost::bind(&ImageDisplayBase::incomingMessage, this, _1));
00181       }
00182     }
00183     setStatus(StatusProperty::Ok, "Topic", "OK");
00184   }
00185   catch (ros::Exception& e)
00186   {
00187     setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
00188   }
00189   catch (image_transport::Exception& e)
00190   {
00191     setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
00192   }
00193 
00194   messages_received_ = 0;
00195   setStatus(StatusProperty::Warn, "Image", "No Image received");
00196 }
00197 
00198 void ImageDisplayBase::unsubscribe()
00199 {
00200   tf_filter_.reset();
00201   sub_.reset(new image_transport::SubscriberFilter());
00202 }
00203 
00204 void ImageDisplayBase::fixedFrameChanged()
00205 {
00206   if (tf_filter_)
00207   {
00208     tf_filter_->setTargetFrame(fixed_frame_.toStdString());
00209     reset();
00210   }
00211 }
00212 
00213 void ImageDisplayBase::scanForTransportSubscriberPlugins()
00214 {
00215   pluginlib::ClassLoader<image_transport::SubscriberPlugin> sub_loader("image_transport",
00216                                                                        "image_transport::SubscriberPlugin");
00217 
00218   BOOST_FOREACH( const std::string& lookup_name, sub_loader.getDeclaredClasses() )
00219   {
00220     // lookup_name is formatted as "pkg/transport_sub", for instance
00221     // "image_transport/compressed_sub" for the "compressed"
00222     // transport.  This code removes the "_sub" from the tail and
00223     // everything up to and including the "/" from the head, leaving
00224     // "compressed" (for example) in transport_name.
00225     std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
00226     transport_name = transport_name.substr(lookup_name.find('/') + 1);
00227 
00228     // If the plugin loads without throwing an exception, add its
00229     // transport name to the list of valid plugins, otherwise ignore
00230     // it.
00231     try
00232     {
00233       boost::shared_ptr<image_transport::SubscriberPlugin> sub = sub_loader.createInstance(lookup_name);
00234       transport_plugin_types_.insert(transport_name);
00235     }
00236     catch (const pluginlib::LibraryLoadException& e)
00237     {
00238     }
00239     catch (const pluginlib::CreateClassException& e)
00240     {
00241     }
00242   }
00243 }
00244 
00245 void ImageDisplayBase::updateTopic()
00246 {
00247   unsubscribe();
00248   reset();
00249   subscribe();
00250   context_->queueRender();
00251 }
00252 
00253 void ImageDisplayBase::fillTransportOptionList(EnumProperty* property)
00254 {
00255   property->clearOptions();
00256 
00257   std::vector<std::string> choices;
00258 
00259   choices.push_back("raw");
00260 
00261   // Loop over all current ROS topic names
00262   ros::master::V_TopicInfo topics;
00263   ros::master::getTopics(topics);
00264   ros::master::V_TopicInfo::iterator it = topics.begin();
00265   ros::master::V_TopicInfo::iterator end = topics.end();
00266   for (; it != end; ++it)
00267   {
00268     // If the beginning of this topic name is the same as topic_,
00269     // and the whole string is not the same,
00270     // and the next character is /
00271     // and there are no further slashes from there to the end,
00272     // then consider this a possible transport topic.
00273     const ros::master::TopicInfo& ti = *it;
00274     const std::string& topic_name = ti.name;
00275     const std::string& topic = topic_property_->getStdString();
00276 
00277     if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/'
00278         && topic_name.find('/', topic.size() + 1) == std::string::npos)
00279     {
00280       std::string transport_type = topic_name.substr(topic.size() + 1);
00281 
00282       // If the transport type string found above is in the set of
00283       // supported transport type plugins, add it to the list.
00284       if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end())
00285       {
00286         choices.push_back(transport_type);
00287       }
00288     }
00289   }
00290 
00291   for (size_t i = 0; i < choices.size(); i++)
00292   {
00293     property->addOptionStd(choices[i]);
00294   }
00295 }
00296 
00297 } // end namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:30