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