marker_display.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 <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   // TODO: this is inefficient, should store every in-use id per namespace and lookup by that
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 // MarkerNamespace
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   // Can't do this connect in chained constructor above because at
00477   // that point it doesn't really know that "this" is a
00478   // MarkerNamespace*, so the signal doesn't get connected.
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 } // namespace rviz
00491 
00492 #include <pluginlib/class_list_macros.h>
00493 PLUGINLIB_EXPORT_CLASS( rviz::MarkerDisplay, rviz::Display )


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