00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00206
00207
00208
00209
00210 std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
00211 transport_name = transport_name.substr(lookup_name.find('/') + 1);
00212
00213
00214
00215
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
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
00254
00255
00256
00257
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
00268
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 }