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   namespace_config_enabled_state_.clear();
00091 }
00092 
00093 MarkerDisplay::~MarkerDisplay()
00094 {
00095   if ( initialized() )
00096   {
00097     unsubscribe();
00098 
00099     clearMarkers();
00100 
00101     delete tf_filter_;
00102   }
00103 }
00104 
00105 void MarkerDisplay::load(const Config& config)
00106 {
00107   Display::load(config);
00108 
00109   Config c = config.mapGetChild("Namespaces");
00110   for( Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() )
00111   {
00112     QString key = iter.currentKey();
00113     const Config& child = iter.currentChild();
00114     namespace_config_enabled_state_[key] = child.getValue().toBool();
00115   }
00116 }
00117 
00118 void MarkerDisplay::clearMarkers()
00119 {
00120   markers_.clear();
00121   markers_with_expiration_.clear();
00122   frame_locked_markers_.clear();
00123   tf_filter_->clear();
00124   namespaces_category_->removeChildren();
00125   namespaces_.clear();
00126 }
00127 
00128 void MarkerDisplay::onEnable()
00129 {
00130   subscribe();
00131 }
00132 
00133 void MarkerDisplay::onDisable()
00134 {
00135   unsubscribe();
00136   tf_filter_->clear();
00137 
00138   clearMarkers();
00139 }
00140 
00141 void MarkerDisplay::updateQueueSize()
00142 {
00143   tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00144 }
00145 
00146 void MarkerDisplay::updateTopic()
00147 {
00148   unsubscribe();
00149   subscribe();
00150 }
00151 
00152 void MarkerDisplay::subscribe()
00153 {
00154   if( !isEnabled() )
00155   {
00156     return;
00157   }
00158 
00159   std::string marker_topic = marker_topic_property_->getTopicStd();
00160   if( !marker_topic.empty() )
00161   {
00162     array_sub_.shutdown();
00163     sub_.unsubscribe();
00164 
00165     try
00166     {
00167       sub_.subscribe( update_nh_, marker_topic, queue_size_property_->getInt() );
00168       array_sub_ = update_nh_.subscribe( marker_topic + "_array", queue_size_property_->getInt(), &MarkerDisplay::incomingMarkerArray, this );
00169       setStatus( StatusProperty::Ok, "Topic", "OK" );
00170     }
00171     catch( ros::Exception& e )
00172     {
00173       setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what() );
00174     }
00175   }
00176 }
00177 
00178 void MarkerDisplay::unsubscribe()
00179 {
00180   sub_.unsubscribe();
00181   array_sub_.shutdown();
00182 }
00183 
00184 void MarkerDisplay::deleteMarker(MarkerID id)
00185 {
00186   deleteMarkerStatus( id );
00187 
00188   M_IDToMarker::iterator it = markers_.find( id );
00189   if( it != markers_.end() )
00190   {
00191     markers_with_expiration_.erase(it->second);
00192     frame_locked_markers_.erase(it->second);
00193     markers_.erase(it);
00194   }
00195 }
00196 
00197 void MarkerDisplay::deleteMarkersInNamespace( const std::string& ns )
00198 {
00199   std::vector<MarkerID> to_delete;
00200 
00201   // TODO: this is inefficient, should store every in-use id per namespace and lookup by that
00202   M_IDToMarker::iterator marker_it = markers_.begin();
00203   M_IDToMarker::iterator marker_end = markers_.end();
00204   for (; marker_it != marker_end; ++marker_it)
00205   {
00206     if (marker_it->first.first == ns)
00207     {
00208       to_delete.push_back(marker_it->first);
00209     }
00210   }
00211 
00212   {
00213     std::vector<MarkerID>::iterator it = to_delete.begin();
00214     std::vector<MarkerID>::iterator end = to_delete.end();
00215     for (; it != end; ++it)
00216     {
00217       deleteMarker(*it);
00218     }
00219   }
00220 }
00221 
00222 void MarkerDisplay::deleteAllMarkers()
00223 {
00224   std::vector<MarkerID> to_delete;
00225   M_IDToMarker::iterator marker_it = markers_.begin();
00226   for (; marker_it != markers_.end(); ++marker_it)
00227   {
00228     to_delete.push_back(marker_it->first);
00229   }
00230 
00231   for (std::vector<MarkerID>::iterator it = to_delete.begin(); it != to_delete.end(); ++it)
00232   {
00233     deleteMarker( *it );
00234   }
00235 }
00236 
00237 void MarkerDisplay::setMarkerStatus(MarkerID id, StatusLevel level, const std::string& text)
00238 {
00239   std::stringstream ss;
00240   ss << id.first << "/" << id.second;
00241   std::string marker_name = ss.str();
00242   setStatusStd(level, marker_name, text);
00243 }
00244 
00245 void MarkerDisplay::deleteMarkerStatus(MarkerID id)
00246 {
00247   std::stringstream ss;
00248   ss << id.first << "/" << id.second;
00249   std::string marker_name = ss.str();
00250   deleteStatusStd(marker_name);
00251 }
00252 
00253 void MarkerDisplay::incomingMarkerArray(const visualization_msgs::MarkerArray::ConstPtr& array)
00254 {
00255   std::vector<visualization_msgs::Marker>::const_iterator it = array->markers.begin();
00256   std::vector<visualization_msgs::Marker>::const_iterator end = array->markers.end();
00257   for (; it != end; ++it)
00258   {
00259     const visualization_msgs::Marker& marker = *it;
00260     tf_filter_->add(visualization_msgs::Marker::Ptr(new visualization_msgs::Marker(marker)));
00261   }
00262 }
00263 
00264 void MarkerDisplay::incomingMarker( const visualization_msgs::Marker::ConstPtr& marker )
00265 {
00266   boost::mutex::scoped_lock lock(queue_mutex_);
00267 
00268   message_queue_.push_back(marker);
00269 }
00270 
00271 void MarkerDisplay::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
00272 {
00273   visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
00274   if (marker->action == visualization_msgs::Marker::DELETE ||
00275       marker->action == 3)  // TODO: visualization_msgs::Marker::DELETEALL when message changes in a future version of ROS
00276   {
00277     return this->processMessage(marker);
00278   }
00279   std::string authority = marker_evt.getPublisherName();
00280   std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
00281   setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
00282 }
00283 
00284 bool validateFloats(const visualization_msgs::Marker& msg)
00285 {
00286   bool valid = true;
00287   valid = valid && validateFloats(msg.pose);
00288   valid = valid && validateFloats(msg.scale);
00289   valid = valid && validateFloats(msg.color);
00290   valid = valid && validateFloats(msg.points);
00291   return valid;
00292 }
00293 
00294 void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr& message )
00295 {
00296   if (!validateFloats(*message))
00297   {
00298     setMarkerStatus(MarkerID(message->ns, message->id), StatusProperty::Error, "Contains invalid floating point values (nans or infs)");
00299     return;
00300   }
00301 
00302   switch ( message->action )
00303   {
00304   case visualization_msgs::Marker::ADD:
00305     processAdd( message );
00306     break;
00307 
00308   case visualization_msgs::Marker::DELETE:
00309     processDelete( message );
00310     break;
00311 
00312   case 3: // TODO: visualization_msgs::Marker::DELETEALL when message changes in a future version of ROS
00313     deleteAllMarkers();
00314     break;
00315 
00316   default:
00317     ROS_ERROR( "Unknown marker action: %d\n", message->action );
00318   }
00319 }
00320 
00321 void MarkerDisplay::processAdd( const visualization_msgs::Marker::ConstPtr& message )
00322 {
00323   QString namespace_name = QString::fromStdString( message->ns );
00324   M_Namespace::iterator ns_it = namespaces_.find( namespace_name );
00325   if( ns_it == namespaces_.end() )
00326   {
00327     ns_it = namespaces_.insert( namespace_name, new MarkerNamespace( namespace_name, namespaces_category_, this ));
00328 
00329     // Adding a new namespace, determine if it's configured to be disabled
00330     if( namespace_config_enabled_state_.count(namespace_name) > 0 &&
00331         !namespace_config_enabled_state_[namespace_name] )
00332     {
00333       ns_it.value()->setValue(false);  // Disable the namespace
00334     }
00335   }
00336 
00337   if( !ns_it.value()->isEnabled() )
00338   {
00339     return;
00340   }
00341 
00342   deleteMarkerStatus( MarkerID( message->ns, message->id ));
00343 
00344   bool create = true;
00345   MarkerBasePtr marker;
00346 
00347   M_IDToMarker::iterator it = markers_.find( MarkerID(message->ns, message->id) );
00348   if ( it != markers_.end() )
00349   {
00350     marker = it->second;
00351     markers_with_expiration_.erase(marker);
00352     if ( message->type == marker->getMessage()->type )
00353     {
00354       create = false;
00355     }
00356     else
00357     {
00358       markers_.erase( it );
00359     }
00360   }
00361 
00362   if ( create )
00363   {
00364     switch ( message->type )
00365     {
00366     case visualization_msgs::Marker::CUBE:
00367     case visualization_msgs::Marker::CYLINDER:
00368     case visualization_msgs::Marker::SPHERE:
00369       {
00370         marker.reset(new ShapeMarker(this, context_, scene_node_));
00371       }
00372       break;
00373 
00374     case visualization_msgs::Marker::ARROW:
00375       {
00376         marker.reset(new ArrowMarker(this, context_, scene_node_));
00377       }
00378       break;
00379 
00380     case visualization_msgs::Marker::LINE_STRIP:
00381       {
00382         marker.reset(new LineStripMarker(this, context_, scene_node_));
00383       }
00384       break;
00385     case visualization_msgs::Marker::LINE_LIST:
00386       {
00387         marker.reset(new LineListMarker(this, context_, scene_node_));
00388       }
00389       break;
00390     case visualization_msgs::Marker::SPHERE_LIST:
00391     case visualization_msgs::Marker::CUBE_LIST:
00392     case visualization_msgs::Marker::POINTS:
00393       {
00394         marker.reset(new PointsMarker(this, context_, scene_node_));
00395       }
00396       break;
00397     case visualization_msgs::Marker::TEXT_VIEW_FACING:
00398       {
00399         marker.reset(new TextViewFacingMarker(this, context_, scene_node_));
00400       }
00401       break;
00402     case visualization_msgs::Marker::MESH_RESOURCE:
00403       {
00404         marker.reset(new MeshResourceMarker(this, context_, scene_node_));
00405       }
00406       break;
00407 
00408     case visualization_msgs::Marker::TRIANGLE_LIST:
00409     {
00410       marker.reset(new TriangleListMarker(this, context_, scene_node_));
00411     }
00412     break;
00413     default:
00414       ROS_ERROR( "Unknown marker type: %d", message->type );
00415     }
00416 
00417     markers_.insert(std::make_pair(MarkerID(message->ns, message->id), marker));
00418   }
00419 
00420   if (marker)
00421   {
00422     marker->setMessage(message);
00423 
00424     if (message->lifetime.toSec() > 0.0001f)
00425     {
00426       markers_with_expiration_.insert(marker);
00427     }
00428 
00429     if (message->frame_locked)
00430     {
00431       frame_locked_markers_.insert(marker);
00432     }
00433 
00434     context_->queueRender();
00435   }
00436 }
00437 
00438 void MarkerDisplay::processDelete( const visualization_msgs::Marker::ConstPtr& message )
00439 {
00440   deleteMarker(MarkerID(message->ns, message->id));
00441 
00442   context_->queueRender();
00443 }
00444 
00445 void MarkerDisplay::update(float wall_dt, float ros_dt)
00446 {
00447   V_MarkerMessage local_queue;
00448 
00449   {
00450     boost::mutex::scoped_lock lock(queue_mutex_);
00451 
00452     local_queue.swap( message_queue_ );
00453   }
00454 
00455   if ( !local_queue.empty() )
00456   {
00457     V_MarkerMessage::iterator message_it = local_queue.begin();
00458     V_MarkerMessage::iterator message_end = local_queue.end();
00459     for ( ; message_it != message_end; ++message_it )
00460     {
00461       visualization_msgs::Marker::ConstPtr& marker = *message_it;
00462 
00463       processMessage( marker );
00464     }
00465   }
00466 
00467   {
00468     S_MarkerBase::iterator it = markers_with_expiration_.begin();
00469     S_MarkerBase::iterator end = markers_with_expiration_.end();
00470     for (; it != end;)
00471     {
00472       MarkerBasePtr marker = *it;
00473       if (marker->expired())
00474       {
00475         ++it;
00476         deleteMarker(marker->getID());
00477       }
00478       else
00479       {
00480         ++it;
00481       }
00482     }
00483   }
00484 
00485   {
00486     S_MarkerBase::iterator it = frame_locked_markers_.begin();
00487     S_MarkerBase::iterator end = frame_locked_markers_.end();
00488     for (; it != end; ++it)
00489     {
00490       MarkerBasePtr marker = *it;
00491       marker->updateFrameLocked();
00492     }
00493   }
00494 }
00495 
00496 void MarkerDisplay::fixedFrameChanged()
00497 {
00498   tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00499 
00500   clearMarkers();
00501 }
00502 
00503 void MarkerDisplay::reset()
00504 {
00505   Display::reset();
00506   clearMarkers();
00507 }
00508 
00509 void MarkerDisplay::setTopic( const QString &topic, const QString &datatype )
00510 {
00511   marker_topic_property_->setString( topic );
00512 }
00513 
00515 // MarkerNamespace
00516 
00517 MarkerNamespace::MarkerNamespace( const QString& name, Property* parent_property, MarkerDisplay* owner )
00518   : BoolProperty( name, true,
00519                   "Enable/disable all markers in this namespace.",
00520                   parent_property )
00521   , owner_( owner )
00522 {
00523   // Can't do this connect in chained constructor above because at
00524   // that point it doesn't really know that "this" is a
00525   // MarkerNamespace*, so the signal doesn't get connected.
00526   connect( this, SIGNAL( changed() ), this, SLOT( onEnableChanged() ));
00527 }
00528 
00529 void MarkerNamespace::onEnableChanged()
00530 {
00531   if( !isEnabled() )
00532   {
00533     owner_->deleteMarkersInNamespace( getName().toStdString() );
00534   }
00535 
00536   // Update the configuration that stores the enabled state of all markers
00537   owner_->namespace_config_enabled_state_[getName()] = isEnabled();
00538 }
00539 
00540 } // namespace rviz
00541 
00542 #include <pluginlib/class_list_macros.h>
00543 PLUGINLIB_EXPORT_CLASS( rviz::MarkerDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15