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 <sstream>
00031
00032 #include <tf/transform_listener.h>
00033
00034 #include "rviz/default_plugin/markers/arrow_marker.h"
00035 #include "rviz/default_plugin/markers/line_list_marker.h"
00036 #include "rviz/default_plugin/markers/line_strip_marker.h"
00037 #include "rviz/default_plugin/markers/mesh_resource_marker.h"
00038 #include "rviz/default_plugin/markers/points_marker.h"
00039 #include "rviz/default_plugin/markers/shape_marker.h"
00040 #include "rviz/default_plugin/markers/text_view_facing_marker.h"
00041 #include "rviz/default_plugin/markers/triangle_list_marker.h"
00042 #include "rviz/display_context.h"
00043 #include "rviz/frame_manager.h"
00044 #include "rviz/ogre_helpers/arrow.h"
00045 #include "rviz/ogre_helpers/billboard_line.h"
00046 #include "rviz/ogre_helpers/shape.h"
00047 #include "rviz/properties/int_property.h"
00048 #include "rviz/properties/property.h"
00049 #include "rviz/properties/ros_topic_property.h"
00050 #include "rviz/selection/selection_manager.h"
00051 #include "rviz/validate_floats.h"
00052
00053 #include "rviz/default_plugin/marker_display.h"
00054
00055 namespace rviz
00056 {
00057
00059
00060 MarkerDisplay::MarkerDisplay()
00061 : Display()
00062 {
00063 marker_topic_property_ = new RosTopicProperty( "Marker Topic", "visualization_marker",
00064 QString::fromStdString( ros::message_traits::datatype<visualization_msgs::Marker>() ),
00065 "visualization_msgs::Marker topic to subscribe to. <topic>_array will also"
00066 " automatically be subscribed with type visualization_msgs::MarkerArray.",
00067 this, SLOT( updateTopic() ));
00068
00069 queue_size_property_ = new IntProperty( "Queue Size", 100,
00070 "Advanced: set the size of the incoming Marker message queue. Increasing this is"
00071 " useful if your incoming TF data is delayed significantly from your Marker data, "
00072 "but it can greatly increase memory usage if the messages are big.",
00073 this, SLOT( updateQueueSize() ));
00074 queue_size_property_->setMin( 0 );
00075
00076 namespaces_category_ = new Property( "Namespaces", QVariant(), "", this );
00077 }
00078
00079 void MarkerDisplay::onInitialize()
00080 {
00081 tf_filter_ = new tf::MessageFilter<visualization_msgs::Marker>( *context_->getTFClient(),
00082 fixed_frame_.toStdString(),
00083 queue_size_property_->getInt(),
00084 update_nh_ );
00085
00086 tf_filter_->connectInput(sub_);
00087 tf_filter_->registerCallback(boost::bind(&MarkerDisplay::incomingMarker, this, _1));
00088 tf_filter_->registerFailureCallback(boost::bind(&MarkerDisplay::failedMarker, this, _1, _2));
00089 }
00090
00091 MarkerDisplay::~MarkerDisplay()
00092 {
00093 if ( initialized() )
00094 {
00095 unsubscribe();
00096
00097 clearMarkers();
00098
00099 delete tf_filter_;
00100 }
00101 }
00102
00103 void MarkerDisplay::clearMarkers()
00104 {
00105 markers_.clear();
00106 markers_with_expiration_.clear();
00107 frame_locked_markers_.clear();
00108 tf_filter_->clear();
00109 namespaces_category_->removeChildren();
00110 namespaces_.clear();
00111 }
00112
00113 void MarkerDisplay::onEnable()
00114 {
00115 subscribe();
00116 }
00117
00118 void MarkerDisplay::onDisable()
00119 {
00120 unsubscribe();
00121 tf_filter_->clear();
00122
00123 clearMarkers();
00124 }
00125
00126 void MarkerDisplay::updateQueueSize()
00127 {
00128 tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00129 }
00130
00131 void MarkerDisplay::updateTopic()
00132 {
00133 unsubscribe();
00134 subscribe();
00135 }
00136
00137 void MarkerDisplay::subscribe()
00138 {
00139 if( !isEnabled() )
00140 {
00141 return;
00142 }
00143
00144 std::string marker_topic = marker_topic_property_->getTopicStd();
00145 if( !marker_topic.empty() )
00146 {
00147 array_sub_.shutdown();
00148 sub_.unsubscribe();
00149
00150 try
00151 {
00152 sub_.subscribe( update_nh_, marker_topic, queue_size_property_->getInt() );
00153 array_sub_ = update_nh_.subscribe( marker_topic + "_array", queue_size_property_->getInt(), &MarkerDisplay::incomingMarkerArray, this );
00154 setStatus( StatusProperty::Ok, "Topic", "OK" );
00155 }
00156 catch( ros::Exception& e )
00157 {
00158 setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what() );
00159 }
00160 }
00161 }
00162
00163 void MarkerDisplay::unsubscribe()
00164 {
00165 sub_.unsubscribe();
00166 array_sub_.shutdown();
00167 }
00168
00169 void MarkerDisplay::deleteMarker(MarkerID id)
00170 {
00171 deleteMarkerStatus( id );
00172
00173 M_IDToMarker::iterator it = markers_.find( id );
00174 if( it != markers_.end() )
00175 {
00176 markers_with_expiration_.erase(it->second);
00177 frame_locked_markers_.erase(it->second);
00178 markers_.erase(it);
00179 }
00180 }
00181
00182 void MarkerDisplay::deleteMarkersInNamespace( const std::string& ns )
00183 {
00184 std::vector<MarkerID> to_delete;
00185
00186
00187 M_IDToMarker::iterator marker_it = markers_.begin();
00188 M_IDToMarker::iterator marker_end = markers_.end();
00189 for (; marker_it != marker_end; ++marker_it)
00190 {
00191 if (marker_it->first.first == ns)
00192 {
00193 to_delete.push_back(marker_it->first);
00194 }
00195 }
00196
00197 {
00198 std::vector<MarkerID>::iterator it = to_delete.begin();
00199 std::vector<MarkerID>::iterator end = to_delete.end();
00200 for (; it != end; ++it)
00201 {
00202 deleteMarker(*it);
00203 }
00204 }
00205 }
00206
00207 void MarkerDisplay::setMarkerStatus(MarkerID id, StatusLevel level, const std::string& text)
00208 {
00209 std::stringstream ss;
00210 ss << id.first << "/" << id.second;
00211 std::string marker_name = ss.str();
00212 setStatusStd(level, marker_name, text);
00213 }
00214
00215 void MarkerDisplay::deleteMarkerStatus(MarkerID id)
00216 {
00217 std::stringstream ss;
00218 ss << id.first << "/" << id.second;
00219 std::string marker_name = ss.str();
00220 deleteStatusStd(marker_name);
00221 }
00222
00223 void MarkerDisplay::incomingMarkerArray(const visualization_msgs::MarkerArray::ConstPtr& array)
00224 {
00225 std::vector<visualization_msgs::Marker>::const_iterator it = array->markers.begin();
00226 std::vector<visualization_msgs::Marker>::const_iterator end = array->markers.end();
00227 for (; it != end; ++it)
00228 {
00229 const visualization_msgs::Marker& marker = *it;
00230 tf_filter_->add(visualization_msgs::Marker::Ptr(new visualization_msgs::Marker(marker)));
00231 }
00232 }
00233
00234 void MarkerDisplay::incomingMarker( const visualization_msgs::Marker::ConstPtr& marker )
00235 {
00236 boost::mutex::scoped_lock lock(queue_mutex_);
00237
00238 message_queue_.push_back(marker);
00239 }
00240
00241 void MarkerDisplay::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
00242 {
00243 visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
00244 std::string authority = marker_evt.getPublisherName();
00245 std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
00246 setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
00247 }
00248
00249 bool validateFloats(const visualization_msgs::Marker& msg)
00250 {
00251 bool valid = true;
00252 valid = valid && validateFloats(msg.pose);
00253 valid = valid && validateFloats(msg.scale);
00254 valid = valid && validateFloats(msg.color);
00255 valid = valid && validateFloats(msg.points);
00256 return valid;
00257 }
00258
00259 void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr& message )
00260 {
00261 if (!validateFloats(*message))
00262 {
00263 setMarkerStatus(MarkerID(message->ns, message->id), StatusProperty::Error, "Contains invalid floating point values (nans or infs)");
00264 return;
00265 }
00266
00267 switch ( message->action )
00268 {
00269 case visualization_msgs::Marker::ADD:
00270 processAdd( message );
00271 break;
00272
00273 case visualization_msgs::Marker::DELETE:
00274 processDelete( message );
00275 break;
00276
00277 default:
00278 ROS_ERROR( "Unknown marker action: %d\n", message->action );
00279 }
00280 }
00281
00282 void MarkerDisplay::processAdd( const visualization_msgs::Marker::ConstPtr& message )
00283 {
00284 QString namespace_name = QString::fromStdString( message->ns );
00285 M_Namespace::iterator ns_it = namespaces_.find( namespace_name );
00286 if( ns_it == namespaces_.end() )
00287 {
00288 ns_it = namespaces_.insert( namespace_name, new MarkerNamespace( namespace_name, namespaces_category_, this ));
00289 }
00290
00291 if( !ns_it.value()->isEnabled() )
00292 {
00293 return;
00294 }
00295
00296 deleteMarkerStatus( MarkerID( message->ns, message->id ));
00297
00298 bool create = true;
00299 MarkerBasePtr marker;
00300
00301 M_IDToMarker::iterator it = markers_.find( MarkerID(message->ns, message->id) );
00302 if ( it != markers_.end() )
00303 {
00304 marker = it->second;
00305 markers_with_expiration_.erase(marker);
00306 if ( message->type == marker->getMessage()->type )
00307 {
00308 create = false;
00309 }
00310 else
00311 {
00312 markers_.erase( it );
00313 }
00314 }
00315
00316 if ( create )
00317 {
00318 switch ( message->type )
00319 {
00320 case visualization_msgs::Marker::CUBE:
00321 case visualization_msgs::Marker::CYLINDER:
00322 case visualization_msgs::Marker::SPHERE:
00323 {
00324 marker.reset(new ShapeMarker(this, context_, scene_node_));
00325 }
00326 break;
00327
00328 case visualization_msgs::Marker::ARROW:
00329 {
00330 marker.reset(new ArrowMarker(this, context_, scene_node_));
00331 }
00332 break;
00333
00334 case visualization_msgs::Marker::LINE_STRIP:
00335 {
00336 marker.reset(new LineStripMarker(this, context_, scene_node_));
00337 }
00338 break;
00339 case visualization_msgs::Marker::LINE_LIST:
00340 {
00341 marker.reset(new LineListMarker(this, context_, scene_node_));
00342 }
00343 break;
00344 case visualization_msgs::Marker::SPHERE_LIST:
00345 case visualization_msgs::Marker::CUBE_LIST:
00346 case visualization_msgs::Marker::POINTS:
00347 {
00348 marker.reset(new PointsMarker(this, context_, scene_node_));
00349 }
00350 break;
00351 case visualization_msgs::Marker::TEXT_VIEW_FACING:
00352 {
00353 marker.reset(new TextViewFacingMarker(this, context_, scene_node_));
00354 }
00355 break;
00356 case visualization_msgs::Marker::MESH_RESOURCE:
00357 {
00358 marker.reset(new MeshResourceMarker(this, context_, scene_node_));
00359 }
00360 break;
00361
00362 case visualization_msgs::Marker::TRIANGLE_LIST:
00363 {
00364 marker.reset(new TriangleListMarker(this, context_, scene_node_));
00365 }
00366 break;
00367 default:
00368 ROS_ERROR( "Unknown marker type: %d", message->type );
00369 }
00370
00371 markers_.insert(std::make_pair(MarkerID(message->ns, message->id), marker));
00372 }
00373
00374 if (marker)
00375 {
00376 marker->setMessage(message);
00377
00378 if (message->lifetime.toSec() > 0.0001f)
00379 {
00380 markers_with_expiration_.insert(marker);
00381 }
00382
00383 if (message->frame_locked)
00384 {
00385 frame_locked_markers_.insert(marker);
00386 }
00387
00388 context_->queueRender();
00389 }
00390 }
00391
00392 void MarkerDisplay::processDelete( const visualization_msgs::Marker::ConstPtr& message )
00393 {
00394 deleteMarker(MarkerID(message->ns, message->id));
00395 context_->queueRender();
00396 }
00397
00398 void MarkerDisplay::update(float wall_dt, float ros_dt)
00399 {
00400 V_MarkerMessage local_queue;
00401
00402 {
00403 boost::mutex::scoped_lock lock(queue_mutex_);
00404
00405 local_queue.swap( message_queue_ );
00406 }
00407
00408 if ( !local_queue.empty() )
00409 {
00410 V_MarkerMessage::iterator message_it = local_queue.begin();
00411 V_MarkerMessage::iterator message_end = local_queue.end();
00412 for ( ; message_it != message_end; ++message_it )
00413 {
00414 visualization_msgs::Marker::ConstPtr& marker = *message_it;
00415
00416 processMessage( marker );
00417 }
00418 }
00419
00420 {
00421 S_MarkerBase::iterator it = markers_with_expiration_.begin();
00422 S_MarkerBase::iterator end = markers_with_expiration_.end();
00423 for (; it != end;)
00424 {
00425 MarkerBasePtr marker = *it;
00426 if (marker->expired())
00427 {
00428 ++it;
00429 deleteMarker(marker->getID());
00430 }
00431 else
00432 {
00433 ++it;
00434 }
00435 }
00436 }
00437
00438 {
00439 S_MarkerBase::iterator it = frame_locked_markers_.begin();
00440 S_MarkerBase::iterator end = frame_locked_markers_.end();
00441 for (; it != end; ++it)
00442 {
00443 MarkerBasePtr marker = *it;
00444 marker->updateFrameLocked();
00445 }
00446 }
00447 }
00448
00449 void MarkerDisplay::fixedFrameChanged()
00450 {
00451 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00452
00453 clearMarkers();
00454 }
00455
00456 void MarkerDisplay::reset()
00457 {
00458 Display::reset();
00459 clearMarkers();
00460 }
00461
00462 void MarkerDisplay::setTopic( const QString &topic, const QString &datatype )
00463 {
00464 marker_topic_property_->setString( topic );
00465 }
00466
00468
00469
00470 MarkerNamespace::MarkerNamespace( const QString& name, Property* parent_property, MarkerDisplay* owner )
00471 : BoolProperty( name, true,
00472 "Enable/disable all markers in this namespace.",
00473 parent_property )
00474 , owner_( owner )
00475 {
00476
00477
00478
00479 connect( this, SIGNAL( changed() ), this, SLOT( onEnableChanged() ));
00480 }
00481
00482 void MarkerNamespace::onEnableChanged()
00483 {
00484 if( !isEnabled() )
00485 {
00486 owner_->deleteMarkersInNamespace( getName().toStdString() );
00487 }
00488 }
00489
00490 }
00491
00492 #include <pluginlib/class_list_macros.h>
00493 PLUGINLIB_EXPORT_CLASS( rviz::MarkerDisplay, rviz::Display )