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 }
00071 
00072 ImageDisplayBase::~ImageDisplayBase()
00073 {
00074   unsubscribe();
00075 }
00076 
00077 void ImageDisplayBase::onInitialize()
00078 {
00079   it_.reset( new image_transport::ImageTransport( update_nh_ ));
00080   scanForTransportSubscriberPlugins();
00081 }
00082 
00083 void ImageDisplayBase::setTopic( const QString &topic, const QString &datatype )
00084 {
00085   if ( datatype == ros::message_traits::datatype<sensor_msgs::Image>() )
00086   {
00087     transport_property_->setStdString( "raw" );
00088     topic_property_->setString( topic );
00089   }
00090   else
00091   {
00092     int index = topic.lastIndexOf("/");
00093     if ( index == -1 )
00094     {
00095       ROS_WARN("ImageDisplayBase::setTopic() Invalid topic name: %s",
00096                topic.toStdString().c_str());
00097       return;
00098     }
00099     QString transport = topic.mid(index + 1);
00100     QString base_topic = topic.mid(0, index);
00101 
00102     transport_property_->setString( transport );
00103     topic_property_->setString( base_topic );
00104   }
00105 }
00106 
00107 
00108 void ImageDisplayBase::incomingMessage(const sensor_msgs::Image::ConstPtr& msg)
00109 {
00110   if (!msg || context_->getFrameManager()->getPause() )
00111   {
00112     return;
00113   }
00114 
00115   ++messages_received_;
00116   setStatus(StatusProperty::Ok, "Image", QString::number(messages_received_) + " images received");
00117 
00118   emitTimeSignal( msg->header.stamp );
00119 
00120   processMessage(msg);
00121 }
00122 
00123 
00124 void ImageDisplayBase::reset()
00125 {
00126   Display::reset();
00127   if (tf_filter_)
00128     tf_filter_->clear();
00129   messages_received_ = 0;
00130 }
00131 
00132 void ImageDisplayBase::updateQueueSize()
00133 {
00134   uint32_t size = queue_size_property_->getInt();
00135   if (tf_filter_)
00136     tf_filter_->setQueueSize(size);
00137 }
00138 
00139 void ImageDisplayBase::subscribe()
00140 {
00141   if (!isEnabled())
00142   {
00143     return;
00144   }
00145 
00146   try
00147   {
00148 
00149     tf_filter_.reset();
00150 
00151     sub_.reset(new image_transport::SubscriberFilter());
00152 
00153     if (!topic_property_->getTopicStd().empty() && !transport_property_->getStdString().empty() )
00154     {
00155       sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(),
00156                       image_transport::TransportHints(transport_property_->getStdString()));
00157 
00158       if (targetFrame_.empty())
00159       {
00160         sub_->registerCallback(boost::bind(&ImageDisplayBase::incomingMessage, this, _1));
00161       }
00162       else
00163       {
00164         tf_filter_.reset( new tf::MessageFilter<sensor_msgs::Image>(*sub_, (tf::Transformer&)*(context_->getTFClient()), targetFrame_, (uint32_t)queue_size_property_->getInt(), update_nh_));
00165         tf_filter_->registerCallback(boost::bind(&ImageDisplayBase::incomingMessage, this, _1));
00166       }
00167     }
00168     setStatus(StatusProperty::Ok, "Topic", "OK");
00169   }
00170   catch (ros::Exception& e)
00171   {
00172     setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
00173   }
00174   catch (image_transport::Exception& e)
00175   {
00176     setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
00177   }
00178 
00179   messages_received_ = 0;
00180   setStatus(StatusProperty::Warn, "Image", "No Image received");
00181 }
00182 
00183 void ImageDisplayBase::unsubscribe()
00184 {
00185   tf_filter_.reset();
00186   sub_.reset(new image_transport::SubscriberFilter());
00187 }
00188 
00189 void ImageDisplayBase::fixedFrameChanged()
00190 {
00191   if (tf_filter_)
00192   {
00193     tf_filter_->setTargetFrame(fixed_frame_.toStdString());
00194     reset();
00195   }
00196 }
00197 
00198 void ImageDisplayBase::scanForTransportSubscriberPlugins()
00199 {
00200   pluginlib::ClassLoader<image_transport::SubscriberPlugin> sub_loader("image_transport",
00201                                                                        "image_transport::SubscriberPlugin");
00202 
00203   BOOST_FOREACH( const std::string& lookup_name, sub_loader.getDeclaredClasses() )
00204   {
00205     // lookup_name is formatted as "pkg/transport_sub", for instance
00206     // "image_transport/compressed_sub" for the "compressed"
00207     // transport.  This code removes the "_sub" from the tail and
00208     // everything up to and including the "/" from the head, leaving
00209     // "compressed" (for example) in transport_name.
00210     std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
00211     transport_name = transport_name.substr(lookup_name.find('/') + 1);
00212 
00213     // If the plugin loads without throwing an exception, add its
00214     // transport name to the list of valid plugins, otherwise ignore
00215     // it.
00216     try
00217     {
00218       boost::shared_ptr<image_transport::SubscriberPlugin> sub = sub_loader.createInstance(lookup_name);
00219       transport_plugin_types_.insert(transport_name);
00220     }
00221     catch (const pluginlib::LibraryLoadException& e)
00222     {
00223     }
00224     catch (const pluginlib::CreateClassException& e)
00225     {
00226     }
00227   }
00228 }
00229 
00230 void ImageDisplayBase::updateTopic()
00231 {
00232   unsubscribe();
00233   reset();
00234   subscribe();
00235   context_->queueRender();
00236 }
00237 
00238 void ImageDisplayBase::fillTransportOptionList(EnumProperty* property)
00239 {
00240   property->clearOptions();
00241 
00242   std::vector<std::string> choices;
00243 
00244   choices.push_back("raw");
00245 
00246   // Loop over all current ROS topic names
00247   ros::master::V_TopicInfo topics;
00248   ros::master::getTopics(topics);
00249   ros::master::V_TopicInfo::iterator it = topics.begin();
00250   ros::master::V_TopicInfo::iterator end = topics.end();
00251   for (; it != end; ++it)
00252   {
00253     // If the beginning of this topic name is the same as topic_,
00254     // and the whole string is not the same,
00255     // and the next character is /
00256     // and there are no further slashes from there to the end,
00257     // then consider this a possible transport topic.
00258     const ros::master::TopicInfo& ti = *it;
00259     const std::string& topic_name = ti.name;
00260     const std::string& topic = topic_property_->getStdString();
00261 
00262     if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/'
00263         && topic_name.find('/', topic.size() + 1) == std::string::npos)
00264     {
00265       std::string transport_type = topic_name.substr(topic.size() + 1);
00266 
00267       // If the transport type string found above is in the set of
00268       // supported transport type plugins, add it to the list.
00269       if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end())
00270       {
00271         choices.push_back(transport_type);
00272       }
00273     }
00274   }
00275 
00276   for (size_t i = 0; i < choices.size(); i++)
00277   {
00278     property->addOptionStd(choices[i]);
00279   }
00280 }
00281 
00282 } // end namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27