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