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